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Euler  angle  filters. 
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estimation  and  development  of  a  system  capable  of  determining  the  orientation  of  an 
object  in  world  coordinates.  System  sensors  were  built  using  miniature  accelerometers, 
rate  sensors,  and  magnetometers.  The  software  system  was  designed  by  using  Unified 
Modeling  Language  (UML)  with  object  oriented  design  techniques.  The  actual 
implementation  created  a  real  time  orientation  tracking  system. 

The  system  was  tested  with  dynamic  tilt  table  experiments.  Test  results  showed 
that  the  quaternion  attitude  estimation  filter  system  can  track  human  limb  segments  in 
real  time  within  1  degree  of  accuracy  in  any  orientation  and  with  a  55  Hz.  update  rate. 
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I.  INTRODUCTION 


A.  MOTIVATION 

For  many  years,  computer  scientists  have  tried  to  create  realistic  virtual 
environments  (VE)  for  simulation,  training,  evaluation,  data  visualization,  computer 
aided  design,  tele-operation,  tele-presence,  robotics  and  entertainment.  During  this 
period,  computer  performance  increased  rapidly  in  both  hardware  and  software 
[HENA97].  Many  techniques  and  devices  have  been  discovered  and  introduced  to 
interface  computers  and  humans.  These  advances  are  needed  to  deal  with  the  well- 
known  “human  machine  interaction  problem”  [HENA97].  Special  interaction  devices  are 
needed  to  achieve  the  goal  of  total  immersion  of  humans  into  virtual  environments 
[HENA97].  The  main  thrust  of  research  in  this  area  has  been  directed  toward  producing 
new  and  improved  sensors  for  tracking  objects  and  humans. 

There  are  currently  five  fundamental  tracking  technologies;  namely,  mechanical, 
electromagnetic,  acoustic,  optical  and  inertial  [DURL95].  Very  recently,  due  to  advances 
in  component  technology,  inertial  tracking  systems  have  become  more  promising  than 
other  tracking  technologies  [SKOP96].  Inertial  tracking  systems  use  a  combination  of 
angular  rate,  accelerometer  and  magnetic  sensors.  Filter  algorithms  combine  these  sensor 
readings  to  obtain  limb  segment  orientations.  Such  trackers  are  free  of  most  of  the 
problems  of  other  tracking  systems. 

B.  PROBLEM  STATEMENT 

Several  methods  exist  for  describing  the  orientation  of  an  object.  The  most 
widely  used  method  is  to  use  Euler  angles  to  describe  attitude.  However,  tracking 
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systems  using  Euler  angles  are  not  capable  of  tracking  objects  in  all  orientations  due  to 
gimbal  lock  singularities  [BACH96,  SKOP96], 

A  quaternion  based  attitude  estimation  filter  has  been  proposed  to  overcome  the 
singularities  and  the  divide  by  zero  errors  encountered  when  using  Euler  angles  to 
represent  orientations  [MCGH96].  The  theory  behind  the  quaternion  attitude  estimation 
filter  has  been  proved  to  be  correct  and  a  simulation  program  has  been  written  in  ANSI 
Common  Lisp  using  only  static  sensor  readings  [HENA97].  The  goal  of  this  thesis  is  to 
develop  a  real-time  quaternion-based  orientation  estimation  filter  in  a  PC  environment, 
quantitatively  test  this  system  with  a  tilt  table,  and  qualitatively  test  it  when  mounted  on  a 
human  limb  segment  [USTA99].  Before  the  real-time  software  development,  white  noise 
effects  on  this  filter  are  examined  with  a  simulation  program.  In  this  thesis,  the  Unified 
Modeling  Language  (UML)  [DOUG98]  is  used  for  software  development,  and  the  C++ 
programming  language  for  system  implementation. 

C.  ORGANIZATION 

Chapter  II  of  this  thesis  surveys  existing  tracking  technology  and  related  work  in 
the  area  of  inertial  tracking  systems.  Chapter  III  discusses  representations  of  rigid  body 
orientation  with  Euler  angles  and  quaternions,  and  presents  a  comparison  between  Euler 
angles  and  quaternions.  Chapter  IV  presents  the  detailed  mathematical  theory  behind  the 
quaternion  attitude  estimation  filter,  white  noise  effects  on  the  filter,  and  the  derivation  of 
the  gain  constant  in  the  filtering  process.  Chapter  V  explains  the  hardware  configuration 
of  the  system.  Chapter  VI  presents  the  software  design  and  development  process. 
Chapter  VII  presents  the  test  results  of  this  real  time  implementation  of  the  filter  with  a 
tilt  table.  The  last  chapter.  Chapter  VIII,  presents  the  conclusions  of  this  research.  It 
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introduces  the  possibility  of  using  this  system  as  an  inertial  tracking  system  in  the  NPS 
Autonomous  Underwater  Vehicle  (AUV)  project  [YUN97]  and  the  body  suit  project 
[ZYDA97].  Recommendations  are  made  for  future  work  and  ways  to  improve  the 
quaternion  attitude  estimation  filter. 
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II.  BACKGROUND 


A.  INTRODUCTION 

A  basic  requirement  in  virtual  environments  (VE)  is  the  tracking  of  objects, 
especially  humans.  Tracking  of  humans  creates  a  convenient  human  machine  interface  to 
the  virtual  environment.  If  the  user  is  to  interact  in  a  natural  way  with  a  virtual 
environment,  then  the  use  of  standard  2D  devices  becomes  unacceptable  [FREY96].  3D 
user  interfaces  in  a  virtual  environment  require  the  use  of  devices  that  are  special  to  that 
environment.  There  are  many  3D  user  interface  systems  available;  however,  they  all 
have  unique  problems  in  real-time  applications.  This  chapter  presents  current  motion 
tracking  devices,  their  usage  in  virtual  environments,  and  their  effectiveness  when 
applied  to  real-time  human  motion  tracking. 

B.  REQUIREMENTS  FOR  SPATIAL  TRACKERS 

Tracking  devices  allow  a  virtual  reality  system  to  display  the  x,  y  and  z  position  and 
the  yaw,  pitch  and  roll  orientation  of  a  tracked  object  or  a  human  body  part.  The  primary 
purpose  of  any  tracking  device  is  to  provide  an  intuitive  interface  between  human  and 
machine  [HENA97].  Human  machine  interfaces  include  all  the  devices  used  in  a  virtual 
environment  system  to  present  information  to  the  human  or  to  sense  the  actions  or 
responses  of  the  human  [DURL95].  Before  selecting  an  appropriate  tracking  device,  it  is 
necessary  to  determine  the  characteristics  and  behaviors  of  the  tracked  objects. 

For  normal  arm  movements  during  reaching,  a  fast  motion  is  accomplished  in 

about  0.5  seconds,  wrist  tangential  velocities  are  about  3  m/s  and  the  accelerations  are 

about  5-6  g.  For  the  fastest  arm  motion  such  as  throwing  a  baseball,  good  pitchers 

release  the  ball  at  37  m/s  and  accelerate  their  hands  at  more  than  25  g.  Motion 
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bandwidths  of  normal  arm  movements  are  around  2  Hz;  the  fastest  hand  motions  are  at 
around  5-6  Hz.  The  frequency  content  of  normal  arm  motion  can  be  defined  as  5  Hz  with 
a  sampling  rate  of  roughly  100  Hz  [DURL95]. 

Head  movements  can  be  as  fast  as  1000  deg/s  in  yaw.  Usual  peak  velocities  are 
about  600  deg/s  for  yaw  and  300  deg/s  for  pitch  and  roll.  Tracker-to-host  reporting  rates 
must  be  at  least  30  Hz.  Delays  of  60  ms  or  more  between  head  motion  and  visual 
feedback  are  known  to  impair  adaptation  and  the  illusion  of  presence.  Much  smaller 
delays  may  cause  simulator  sickness  [DURL95]. 

C.  MOTION  TRACKER  PERFORMANCE 

Several  different  tracking  devices  and  technologies  have  been  developed  and 
applied  to  virtual  environment  applications.  [MEYE92,  SKOP96,  HENA97]  suggest 
some  key  measures  by  which  tracking  systems  may  be  evaluated,  namely; 

•  resolution  and  accuracy 

•  responsiveness 

•  robustness 

•  registration 

•  sociability 

Resolution  can  be  defined  as  the  smallest  change,  which  can  be  detected  by  a 
given  tracking  system  [HENA97].  Accuracy  is  the  sensor  error  range.  The  precision 
with  which  actions  can  be  executed  in  the  virtual  world  depends  on  the  resolution  and 
accuracy  of  a  tracking  device  used.  The  range  of  a  tracking  device  is  the  maximum 
distance  between  the  sensor  and  source  up  to  which  the  position  and  the  orientation  can 
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be  measured  with  a  specified  error  [BARA93].  Accuracy  would  also  include  sensor  drift ; 
i.e.,  the  tendency  of  output  to  change  without  any  change  in  input  [SKOP96]. 

Responsiveness  is  a  measure  of  the  quickness  with  which  new  information  is 
provided.  It  is  determined  by  sampling  rate,  data  rate,  update  rate,  and  latency 
[SKOP96].  Sampling  rate  is  simply  how  often  the  sensor  is  checked  for  new  data.  Data 
rate  is  defined  as  the  number  of  computed  data  points  per  second  that  the  system  can 
provide.  Many  systems  will  implement  a  much  higher  sampling  rate  than  data  rate  in 
order  to  assure  that  new  data  is  not  missed.  Filtering  the  sensor  data  takes  time  and  will 
hinder  real  time  updates.  The  rate  at  which  the  system  can  provide  updated  position  and 
orientation  data  to  the  host  computer  is  the  update  rate.  Latency  is  perhaps  the  most 
important  characteristic  of  responsiveness.  The  usefulness  of  tracking  devices  in  virtual 
environments  depends  to  a  large  degree  on  whether  the  computer  can  track  the 
movements  of  the  user  fast  enough  to  keep  the  virtual  world  synchronized  with  the  user’s 
actions.  This  ability  is  determined  by  the  lag  of  the  signal,  and  the  sensor’s  update  rate. 
The  signal  lag  is  the  delay  in  time  between  a  change  of  the  position  and  orientation  of  the 
target  being  tracked  and  the  report  of  the  change  to  the  computer.  Lags  above  50  ms  are 
perceptible  to  the  user  and  affect  human  performance.  Typical  update  rates  are  between 
30  and  60  updates  per  second  [BARA93].  A  system’s  lag  is  sometimes  referred  to  as  its 
latency  and  is  one  of  the  most  important  specifications  of  a  tracking  system  [HENA97]. 

Robustness  is  a  measure  of  the  tracker’s  effectiveness  in  the  presence  of  noise  or 
other  signal  interference  in  the  operating  environment  [SKOP96].  Types  of  interference 
include  physical,  metallic,  electronic  and  acoustic  [HENA97].  Depending  on  the 
technology  used,  sensors  may  be  sensitive  to  metal  objects,  radiation  from  display 
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monitors,  and  various  noise  sources  or  objects  coming  between  source  and  sensor.  This 
interference  can  limit  the  effectiveness  of  tracking  devices  [BARA93]. 

Registration  is  the  correspondence  between  a  unit’s  actual  position  and 
orientation  and  its  reported  position  and  orientation  over  the  domain  of  the  working 
volume  [SKOP96]. 

Finally,  sociability  describes  a  system’s  maximum  range  of  operation,  its  working 
volume,  and  the  ability  to  track  multiple  targets  within  that  operating  range.  The  working 
volume  is  that  volume  in  which  the  tracker  can  accurately  report  position  and/or 
orientation  information  [MEYE92,  HENA97].  In  addition  to  considering  these 
performance  factors,  one  might  consider  availability ,  cost  and  ease  of  use  before  actually 
selecting  a  tracking  system  [SKOP96]. 

D.  TYPES  OF  MOTION  TRACKERS 

Most  currently  used  tracking  devices  are  active;  that  is,  a  generated  source  is 
attached  to  the  object  to  be  tracked  or  sensed  by  devices  on  the  tracked  object.  In  passive 
tracking,  the  target  is  monitored  from  a  distance  by  one  or  several  cameras  [BARA93]. 
Current  tracking  devices  are  based  on  electromagnetic,  acoustic,  mechanical,  optical  and 
InfraRed  (IR)  technologies. 

1.  Mechanical  Tracking  Devices 

Mechanical  trackers  measure  changes  in  position  and  orientation  by  using  jointed 
linkages  directly  connected  to  a  point  of  reference.  For  body  motion  tracking,  the  point 
of  reference  can  either  be  another  part  of  the  human  body  or  a  fixed  surface  near  the 
human  [SKOP96].  These  trackers  can  be  separated  into  two  basic  types,  body  based  (ex- 
oskeletal,  goniometer  systems)  and  ground-based  systems  [DURL95].  Body  based 
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systems  are  used  to  track  the  user’s  joint  angles  or  end-effector  positions  relative  to  some 
other  part  of  the  body.  Ground-based  systems  are  attached  to  some  surface  near  the  user. 
Generally,  the  user  grasps  an  implement  whose  position  and  orientation  are  tracked 
[SKOP96]. 

Since  no  external  source  is  required,  these  sensors  are  not  susceptible  to  external 
interference  [HENA97],  and  are  much  less  sensitive  to  their  immediate  environment  than, 
say,  electromagnetic  trackers  [HAND93].  The  lag  for  mechanical  trackers  is  very  short, 
less  than  5  ms,  their  update  rate  is  fairly  high,  300  updates  per  second,  and  they  are  very 
accurate  [BARA93].  The  physical  linkages  are  well  suited  for  providing  haptic 
responses.  Haptic  responses  are  force  feedback  cues  that  enable  the  user  to  experience 
simulated  exertion  forces  during  a  virtual  environment  simulation.  These  cues  further 
enhance  the  realism  of  the  environment  and  the  immersion  of  the  user  [HENA97]. 

The  fact  that  mechanical  trackers  are  a  system  of  physical  linkages  attached  to  the 
body  or  constantly  held  makes  them  cumbersome.  The  main  disadvantage  of  mechanical 
trackers  is  that  the  user’s  motion  is  constrained  by  mechanical  devices.  These  devices 
have  a  restrictive  working  volume  and  are  usually  not  portable.  They  require  a 
designated  area  for  their  use  [HENA97], 

Since  they  have  moving  parts,  mechanical  trackers  can  wear  out  after  a  period  of 
time  [HAND93].  Mechanical  trackers  tend  to  be  accurate,  responsive,  robust  and 
inexpensive,  but  they  have  poor  sociability  [MEYE92,  SKOP96]  and  can  be  difficult  and 
time  consuming  to  calibrate  [DURL95,  PRAT94,  SKOP96].  Applications  requiring  a 
limited  range  of  motion  and  where  user  immobility  is  not  a  problem  are  best  suited  for 
this  type  of  tracking  system  [MEYE92,  HENA97]. 
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2.  Electromagnetic  Tracking  Devices 

Electromagnetic  trackers  utilize  artificially  generated  signals  from  the 
electromagnetic  spectrum.  In  this  case,  the  electromagnetic  spectrum  is  defined  in  a 
narrow  sense  to  mean  radio  and  microwave  frequencies  [SKOP96]. 

An  electromagnetic  tracker  comprises  a  transmitter  and  a  receiver.  A  fluctuating 
magnetic  field  generated  in  the  three  orthogonal  coils  of  a  transmitter  is  picked  up  by 
three  corresponding  coils  in  the  receiver.  The  variations  in  the  received  signal  can  be 
used  to  calculate  the  relative  position  and  orientation  of  the  receiver  and  transmitter.  The 
fluctuating  magnetic  field  may  be  Alternating  Current  or  Direct  Current  [HAND93]. 

An  alternative  method  recently  proposed  involves  use  of  spread-spectrum  ranging 
techniques  [BEBL95,  SKOP96].  This  technique  uses  the  measured  time  of  flight  of 
electromagnetic  pulses  to  a  receiver  to  determine  range  from  a  set  of  fixed  transmitters. 
The  concept  is  similar  to  that  of  the  Global  Positioning  System  (GPS).  A  minimum  of 
three  transmitters  would  be  required  to  determine  position  via  triangulation.  A  fourth 
transmitter  would  be  required  to  ensure  time  can  be  accurately  computed  by  the  receiver. 
Transmitted  signals  would  all  occupy  the  same  wide  bandwidth  and  could  utilize  code 
division  multiple  access  (CDMA)  to  preclude  mutual  interference  [SKOP96]. 

Electromagnetic  trackers  have  been  commercially  available  for  some  time  and  are 
relatively  inexpensive  and  easy  to  use  [SKOP96].  These  devices  are  generally  very 
flexible  due  to  the  small  size  of  the  receiver  (smaller  than  a  1”  cube).  Although  the 
working  volume  is  generally  not  very  large,  a  few  feet,  it  is  usually  possible  to  arrange 
various  combinations  of  time-multiplexed  transmitters  and  receivers  to  cover  more  space 
and  track  more  objects  [HAND93].  These  systems  tend  to  have  good  accuracy  in  a  small 
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working  space,  with  accuracy  trailing  off  as  distances  from  the  transmitter  increase 
[SKOP96]. 

Electromagnetic  trackers  suffer  from  several  sources  of  error.  Electromagnetic 
interference  (EMI)  from  devices  such  as  radios  or  display  units  can  cause  erroneous 
readings.  Large  objects  made  of  ferrous  metals  can  interfere  with  the  electromagnetic 
field,  again  causing  inaccuracies  [HAND93]. 

Robustness  is  adversely  effected  by  sensitivity  to  ferromagnetic  objects  in  the 
vicinity,  with  alternating  current  based  trackers  being  more  suspectible  than  direct  current 
based  trackers.  Alternating  current  systems  tend  to  generate  eddy  currents  in  metallic 
objects,  which  then  cause  their  own  electromagnetic  interference  [SKOP96]. 

Adding  power  to  the  transmitter  to  increase  the  working  volume  can  increase 
noise.  Both  AC  and  DC  systems  are  adversely  impacted  by  noise  from  power  sources. 
Responsiveness  is  poor  compared  to  other  methods  [SKOP96]. 

Latency  is  a  serious  problem  with  electromagnetic  trackers.  Up  until  recently  the 
typical  latency  for  an  electromagnetic  device  was  around  100  ms.  This  is  mostly  due  to 
the  filtering  being  performed  on  the  data  to  remove  noise.  Newer  versions  of  these 
devices  have  attempted  to  address  this  problem  [HAND93].  Sociability  is  best  in  an 
environment  without  ferromagnetic  occlusions,  but  is  limited  due  to  a  small  range  of 
operation.  Still,  these  systems  can  be  very  effective  at  shorter  ranges  [SKOP96]. 
Electromagnetic  tracking  works  best  in  applications  which  require  a  limited  working 
environment  that  is  free  of  electromagnetic  interference. 
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3.  Acoustic  Tracking  Devices 

Acoustic  devices  are  sometimes  called  “ultrasonic  trackers”.  Acoustic  tracking  is 
a  fairly  simple  and  well-understood  technique  [HAND93].  Such  devices  use  high 
frequency  ultrasonic  sound  waves  for  measuring  the  position  and  orientation  of  the  target 
object  by  either  phase-coherence  tracking  or  time-of-flight  (TOF)  tracking  [BARA93]. 

Phase  coherence  tracking  works  by  measuring  the  difference  in  phase  between 
sound  waves  emitted  by  a  transmitter  on  the  target  and  those  emitted  by  a  transmitter  at  a 
known  reference  point.  Time-of-flight  works  by  measuring  the  amount  of  time  that  it 
takes  for  sound  emitted  by  transmitters  on  the  target  to  reach  sensors  located  at  fixed 
positions  in  the  environment.  The  transmitters  emit  sound  at  known  times  and  only  one 
transmitter  is  active  at  a  time  [BARA93].  One  transmitter  and  three  sensors  are  required 
for  3  DOF  while  three  transmitters  and  three  sensors  are  necessary  for  full  6  DOF 
tracking  [HAND93], 

Simple  acoustic  tracking  devices  can  be  constructed  at  low  cost.  They  offer  better 
range  of  operation  than  magnetic  systems  but  can  suffer  severe  effects  from  shadowing 
that  can  occur  when  tracked  body  parts  are  blocked  by  other  objects  [SKOP96].  Objects 
that  move  farther  than  half  a  wavelength  in  one  update  period  will  induce  tracking  error. 
The  speed  of  sound  in  air  varies  with  air  temperature,  pressure  and  humidity.  Hence, 
calculations  of  distance  may  be  incorrect  due  to  environmental  conditions  unless  steps  are 
taken  to  account  for  these  [HAND93].  Time-of-flight  trackers  typically  suffer  from  a  low 
update  rate,  brought  about  by  the  low  speed  of  sound  in  air  [BARA93].  Another 
common  problem  is  that  echoes  of  the  sound  signal  will  be  reflected  from  acoustically 
“hard”  surfaces,  such  as  office  walls,  causing  reception  of  “ghost”  pulses  at  the  sensor 
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and  interference  with  other  transmitted  pulses  [HAND93].  Time-of-flight  tracking 
devices  are  vulnerable  to  spurious  noise  sources  at  any  range  [HENA97]. 

Phase-coherent  systems  enjoy  many  benefits  over  time-of-flight  systems  due  to 
much  higher  data  rates  [MEYE92],  If  the  range  is  small,  both  systems  offer  good 
accuracy,  responsiveness,  and  robustness.  As  range  increases,  data  rates  for  time-of- 
flight  systems  decreases,  causing  responsiveness  and  robustness  to  decrease.  Sociability 
of  phase-coherent  systems  is  better  than  that  of  time-of-flight  systems  due  to  larger 
working  volumes  [MEYE92], 

4.  Optical  Tracking  Devices 

There  are  a  variety  of  approaches  to  optical  sensing  for  position  tracking  and 
mapping.  Distance  may  be  measured  by  triangulation,  by  time-of-flight  or  by 
interferometry.  The  passive  light  of  the  environment  may  be  employed,  structured  light 
may  be  projected,  light  may  be  pulsed,  or  active  or  passive  markers  may  be  placed  on  a 
moving  body.  The  different  types  of  optical  trackers  can  be  broken  into  five  categories; 
passive  stereo  vision  systems,  marker  systems,  structured  light  systems,  laser  radar 
systems,  and  laser  interferometric  systems  [DURL95]. 

Passive  stereo  vision  systems  employ  ambient  light  and  square-array  charge- 
coupled  device  (CCD)  cameras  [DURL95].  Multiple  images  from  cameras  with  varying 
viewpoints  are  compared.  Triangulation  is  then  used  to  determine  position  [SKOP96]. 
An  essential  issue  is  to  solve  the  correspondence  problem  of  relating  the  same  points  in 
two  different  images.  Passive  stereo  vision  systems  are  unlikely  to  be  useful  in  virtual 
environment  in  the  near  term,  as  robustness  and  accuracy  are  not  yet  comparable  to  active 
ranging  systems  [DURL95]. 
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The  stereo  correspondence  problem  is  solved  in  marker  systems  because  a  few, 
easily  identifiable  fiducial  points  are  tracked  on  a  moving  body.  To  create  very  bright 
spots  on  the  image  a  number  of  infrared  light  emitting  diodes  (IRED)  are  used.  For 
detection,  1cm2  position  sensing  detectors  (PSD),  also  called  lateral  effect  photodiodes, 
are  used.  Multiple  markers  can  be  tracked  to  yield  orientation  and  to  follow  multiple 
bodies  simultaneously.  Workspace  volume  is  about  lm3.  A  fundamental  problem  with 
the  use  of  PSD  is  reflection  of  IRED  light  from  environmental  surfaces  [DURL95]. 

Structured  light  systems  use  a  ray,  plane  of  light,  or  a  laser,  to  sweep  across  a 
scene.  At  each  position  of  the  plane,  a  light  stripe  is  created,  which  is  sensed  by  a  two 
dimensional  camera.  The  intersection  of  the  known  plane  and  the  line  of  sight  from  the 
camera  determines  the  three-dimensional  coordinates.  Another  common  method  uses 
laser  spot  scanning  of  the  scene.  In  this  method  either  all  or  only  portions  of  the  scene 
may  be  scanned  for  data  [DURL95]. 

Two  of  the  most  prevalent  techniques  that  use  lasers  include  laser  radar  and  laser 
interferometry  techniques.  Laser  radar  works  in  the  same  way  as  acoustic  ranging 
techniques,  except  that  much  higher  data  rates  are  possible.  Techniques  include  both 
time-of-flight  and  phase  shift.  By  scanning  the  entire  scene,  a  three  dimensional  picture 
of  the  scene  can  be  generated.  Laser  radar  techniques  are  more  appropriate  for  longer 
distances  than  laser  techniques  that  use  triangulation  [DURL95]. 

Laser  interferometry,  uses  a  steered  laser  beam  to  track  a  reflector  on  the  object 
being  tracked.  Phase-shift  ranging  and  angular  information  from  the  steered  system  are 
used  to  determine  position.  An  alternate  method  uses  several  lasers  to  track  the  reflector 
from  different  fixed  positions  to  obtain  range  information.  In  this  case,  the  intersection  of 
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spheres  whose  radii  are  determined  from  the  range  information  determines  the  location  of 
the  point  being  tracked.  The  problem  with  these  techniques  is  that  they  provide  only 
incremental  displacement  data  and  loss  of  signal  via  shadowing  can  be  cause  for  re¬ 
calibration  [DURL95]. 

5.  Inertial  Tracking  Devices 

Inertial  tracking  systems  use  a  combination  of  linear  acceleration,  angular  rate  and 
magnetic  sensors  to  determine  rigid  body  orientation.  Angular  orientation  is  determined 
by  integrating  the  output  from  the  angular  rate  sensors.  Angular  rate  sensors  operate  by 
using  the  differential  combination  of  the  outputs  of  two  vibrating  linear  accelerometers. 
Angular  rate  sensor  output  has  an  error  called  drift.  Drift  is  defined  as  the  tendency  of 
bias  errors,  inherent  to  the  sensor,  to  cause  increasing  orientation  estimation  errors  over 
time.  This  fundamental  limitation  makes  angular  rate  sensors  only  a  relatively  short-term 
solution  to  determining  a  rigid  body’s  spatial  orientation  [HENA97]. 

In  order  to  compensate  for  the  long-term  errors  introduced  by  the  use  of  angular 
rate  sensors,  inertial  systems  utilize  linear  acceleration  sensors  called  accelerometers. 
Accelerometers  measure  the  gravity  vector,  relative  to  the  object  being  tracked  as  well  as 
the  forced  linear  accelerations  of  the  attached  rigid  body. 

The  third  component  of  inertial  systems  is  the  magnetometer.  The  magnetometer 
is  sensitive  to  the  Earth’s  magnetic  field  and  can  sense  rotations  about  the  local  vertical 
axis.  Magnetometers  must  be  used  to  correct  drift  errors  in  the  horizontal  plane 
[HENA97]. 

Originally  these  devices  were  used  in  guidance  systems  for  airplanes  and  missiles 
and  as  such  were  cumbersome  [HAND93].  Several  companies  have  begun 
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manufacturing  small  devices,  but  a  complete  system  is  not  commercially  available  yet  for 
body  tracking.  These  systems  hold  much  promise  for  future  application  to  the  body 
tracking  problem.  Larger  systems  have  shown  that  accuracy,  resolution,  response, 
robustness  and  registration  requirements  for  human  body  tracking  can  be  met  by  this 
technology.  Although  the  technology  is  currently  expensive,  it  is  expected  that  costs  will 
come  down  as  devices  are  marketed.  The  greatest  benefit  of  this  type  of  system  is  its 
sociability.  Whereas  electromagnetic,  acoustic  and  optic  devices  all  require  emissions 
from  a  source  to  track  objects,  inertial  tracking  systems  are  sourceless.  This  precludes 
the  inevitable  disastrous  effects  of  occlusions  and  noise  [SKOP96].  Pure  inertial  systems 
are  capable  of  tracking  only  orientation.  A  hybrid  system  providing  3D  location  of  one 
reference  point  on  the  body  would  be  required  to  fully  solve  the  human  body  tracking 
problem. 

E.  RELATED  WORK 

Various  methods  exist  for  tracking  the  rotations  of  an  object  and  representing 
these  rotations  in  a  virtual  environment.  Orientation  filters  based  on  Euler  Angles  are  not 
capable  of  tracking  orientations  through  the  vertical,  due  to  the  gimbal  lock  singularities 
and  divide  by  zero  errors  in  equations  involving  trigonometric  functions  [BACH97B]. 

A  specific  example  of  an  orientation  filter  based  on  Euler  Angles  is  the  navigation 
filter  used  in  the  NPS  AUV  project  [MCGH95,  BACH96].  The  filter  incorporates  inputs 
from  an  onboard  Inertial  Measuring  Unit  (IMU),  a  compass,  and  a  water- speed  sensor. 
Intermittent  GPS  fixes  periodically  provide  accurate  real-time  navigational  data 
[BACH96,  HENA97].  A  schematic  representation  of  the  attitude  estimation  part  of  the 
SANS  navigation  filter  is  shown  in  Figure  1. 
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Figure  1:  Euler  Angle  Attitude  Filter  [HENA97] 

The  SANS  filter  experiences  singularities  in  two  separate  places.  The 

accelerometer  estimate  of  the  roll  angle  and  the  angular  rates  of  roll  and  azimuth  become 
undefined  at  elevation  angles  of  ±90  degrees. 

In  past  research,  [SKOP96]  Euler  Angles  were  used  to  track  and  to  represent  the 
human  upper  body.  SKOP96  was  forced  to  employ  error-checking  programming 
techniques  in  the  software  to  avoid  the  singularities  of  an  Euler  Angle  based  orientation 
filter.  He  concluded  that  current  electromagnetic  trackers  lack  sufficient  accuracy  and 
registration  to  enable  their  use  as  a  true  six  degree  of  freedom  (DOF)  tracker  in  human 
body  tracking  applications  and  called  for  the  investigation  of  new  tracking  technologies 
to  support  the  insertion  of  dismounted  infantry  into  a  virtual  environment  [ZYDA97]. 

In  order  to  avoid  singularities  in  human  body  tracking  or  in  a  navigation  system, 
an  alternative  representation  is  needed  for  orientation  filters.  The  quaternion  attitude 
estimation  filter  was  proposed  by  [MCGH96]  as  an  alternative  representation  and 
improvement  to  the  filters  based  on  Euler  Angles. 
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HENA97  developed  the  software  necessary  to  support  simulation  of  a  quaternion 
filter  using  inertial  sensors.  His  software  was  written  in  ANSI  Common  Lisp.  The  filter 
was  tested  with  a  computer  simulated  inertial  tracker  and  used  only  static  sensor  readings. 
This  research  showed  that  the  theory  behind  the  quaternion  attitude  estimation  filter  was 
sound.  Reduced  computational  complexity  was  achieved  since  the  quaternion  filter  uses 
no  trigonometric  functions. 

F.  SUMMARY 

Excepting  inertial  and  spread- spectrum  systems,  the  systems  described  in  this 
chapter  are  currently  available  [SKOP96].  Mechanical  techniques  for  tracking  the  upper 
body  have  been  implemented  and  have  been  shown  to  be  cumbersome  and  difficult  to 
use.  Acoustic  trackers  can  provide  potentially  excellent  accuracy  and  resolution  together 
with  a  greater  range  of  operation  than  magnetic  trackers.  Hybrid  spread-spectrum 
ranging  and  inertial  tracking  systems  have  potential  for  providing  increases  in  accuracy, 
response  and  range  of  operation  over  systems  available  now  [SKOP96]. 

This  chapter  presented  a  brief  overview  of  current  tracking  technologies  and  past 
research  into  body  tracking  systems  and  orientation  filters.  More  detailed  information 
about  tracking  systems  can  be  found  in  [DURL95],  The  next  chapter  presents 
representations  of  rigid  body  motion  with  Euler  angles  and  quaternions,  and  a 
comparison  between  Euler  angles  and  quaternions. 
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III.  RIGID  BODY  ROTATION 


A.  INTRODUCTION 

Computer  animation  and  robotics  both  involve  manipulating,  rotating  and 
translating  moving  objects  in  a  3D  environment.  Several  different  systems  are  used  to 
describe  positions  and  motions  in  space.  These  include  Euler  angles  and  quaternions. 
Each  of  these  has  particular  advantages  and  disadvantages.  This  chapter  will  provide  the 
basic  information  about  Euler  Angles  and  Quaternions. 

B.  EULER  METHOD 

The  most  popular  method  for  describing  orientation  in  Earth  coordinates  is  the 
Euler  Method.  Using  a  sequence  of  three  angles,  the  Euler  Method  provides  an  intuitive 
description  of  attitude.  Although  eleven  other  possibilities  exist,  these  angles  typically 
consist  of  the  familiar  azimuth  angle,  \|/ ,  the  elevation  angle,  0 ,  and  the  roll  angle,  <p , 
[CRAI89].  Euler  angles  specify  three  successive  rotations  to  bring  the  Earth  coordinates 
into  alignment  with  the  body  coordinates  [COOK92]. 

1.  Rigid  Body  Rotation  with  Euler  Angles 

Euler’s  theorem  states  that  any  numbers  of  rotations  of  a  rigid  body  about  an 
Earth-fixed  axis  are  equivalent  to  a  single  rotation  about  a  single  Earth-fixed  axis.  If  all 
rotations  are  about  the  north(x),  east(y)  and  down(z)  axes,  as  depicted  in  Figure  2,  the 
angles  are  called  Euler  Angles. 
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Figure  2:  Right  Handed  Earth  Coordinate  Axis 

As  stated  previously,  there  are  twelve  sets  of  rotations.  The  standard  “azimuth. 


elevation,  and  roll”  set  is  represented  by  the  three  rotation  matrices  given  in  Eq.  3.1.  Rx 
represents  a  rotation  about  the  x  axis  [CRAI89].  Similarly  for  Rz  and  Ry. 

R  =  RzRyRx  (Eq.  3.1) 


These  names  above  and  the  symbols  \|/,0,  and  tp  are  reserved  for  the  Euler  angle 
set.  The  sign  of  a  rotation  is  determined  using  the  right  hand  rule.  The  ranges  for  the 
rotation  angles  are 


y/  =  ±7t 


0  =  ± 


K 


(p  =  ±n 


(Eq.  3.2) 


The  x-axis  rotation  matrix  (roll)  is  given  by  [CRAI89] 


Rx((p)  = 


1  0  0 

0  coscp  -  sin9 

0  sintp  costp 


The  y-axis  rotation  matrix  (elevation,  pitch)  is  given  by 

Ry  {0)  = 


cos  6 

0 

sin# 

0 

1 

0 

-sin# 

0 

cos# 

(Eq.  3.3) 


(Eq.  3.4) 
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The  z-axis  rotation  matrix  (azimuth,  heading,  yaw)  is  given  by 


Rz(V0  = 


cos  y/  -  sin  ^  0 
sin^  cos^  0 
0  0  1 


(Eq.  3.5) 


In  general,  every  rotation  matrix  is  a  3x3  matrix,  but  to  make  both  translation  and 
rotation  calculations  using  matrices,  similar  4x4  homogeneous  matrices  are  used.  A 
homogeneous  rotation  matrix  in  graphics,  takes  the  form  [FOLE97] 


R  = 


r  r  r  0 
r  r  r  0 
r  r  r  0 
0  0  0  1 


(Eq.  3.6) 


Any  rotation  matrix  can  be  put  into  this  general  form.  Rotations  of  an  object  are 
made  by  multiplying  these  matrices  with  the  homogenous  coordinates  of  the  object.  In 
composing  the  results  of  successive  rotations  by  matrix  multiplication,  the  first  rotation  is 
associated  with  the  right-most  matrix.  Depending  on  the  order  of  the  rotations,  there 
exists  more  than  one  way  to  describe  a  given  orientation. 

Any  rotation  can  be  calculated  by  multiplying  these  individual  matrices  in  the 
right  order.  For  example  a  roll  rotation,  followed  by  an  elevation  and  followed  by  a 
change  in  azimuth  would  be  calculated  by  multiplying  the  matrices  from  right  to  left 

R  =  RzRyRx  (Eq.  3.7) 

resulting  in 


R  = 


cos  y/cos9 
siny/cosd 
-sin  6 


sin  <psin  6cos  y/  -  cos  ^sin  y/  sin  <psin  y/ + cos  (pcos  y/sin9 

sin  <psin  ftin  y/ + cos  (pcos  yr  cos  cp sin  #sin  yr  -  cos  y/  sin  (p 
cos6s'm(p  cos  (pcos  6 


(Eq.3.8) 
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Figure  3:  Body  Coordinate  Axes 
2.  Body  Rates  to  Euler  Rates 

To  rotate  a  body,  its  Rotational  Velocity  around  the  nose  vector  (n),  the  side 
vector  (s)  and  the  approach  or  belly  vector  (a)  can  be  calculated.  Figure  3.  This  rotational 
velocity  can  be  presented  in  Earth  coordinates  as 

Eco  -  (Oy  (Ecj.  3.9) 

and  in  body  coordinates 

Bffl=[pqr]T  (Eq.  3.10) 

where  p  is  roll  rate,  q  is  pitch  rate  and  r  is  yaw  rate.  These  words  and  symbols  are 
reserved  for  body  rotations  [MCGH93].  Euler  rates  are  in  Earth  coordinates  and  p,  q,  and 
r  are  in  body  coordinates.  Thus,  the  following  must  be  noted. 

•  Roll  rate,  p  ^  roll  Euler  angle  rate,  <p 

•  Pitch  rate,  q  ^  elevation  Euler  angle  rate,  0 

•  Yaw  rate,  r  £  azimuth  Euler  angle  rate,  y/ 
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The  rotation  rate  in  Earth  coordinates  is; 


V 

O' 

<i> 

0 

+  RZ 

0 

+ 

& 

N 

0 

y. 

0 

_o_ 

Evidently,-  the  rotation  in  Body  coordinates  is; 


Rco  ~ 


RzRyRx  Eco  ~  RzRyRx 


' a ) 


(Eq.  3.11) 


(Eq.  3.12) 


Since 


= */ V V  Ew = vVV‘4 


o" 

'o' 

<p 

*xTRyW 

0 

¥_ 

+  R/RyT 

# 

0 

+  R/ 

0 

0_ 

= 


cos0 

sin^sin# 

cos^in# 


0 

cos  (p 
-sin^? 


-sin# 

sin^cos# 

cos^cos# 


it  follows  that 


Likewise, 


and 


R/R/ 


'o' 

0 

# 

=  # 

cos<p 

0 

—  sin  (p 

RxTRyTRzT 


0 

0 

V 


v\ 


-sin# 

sin^cos# 

cosmos# 


RxT 


<f> 

V 

0 

=  9 

0 

0 

0 

(Eq.  3.13) 


(Eq.  3.14) 


(Eq.  3.15) 


(Eq.  3.16) 


(Eq.  3.17) 


(Eq.  3.18) 
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Thus 


-  sin0 

0 

V 

•®co  ~  V 

sin  (p  cos  0 

+  0 

coscp 

+  9 

0 

coscpcosQ 

-sincp 

0 

=  {pqrY 


and 


-\j/sin0  +  (p 

P 

5(0  - 

\j/ sincp  cos  0  +  0cos(p 

= 

q 

\j/ cos  cp  cos  0  -  0  sin  cp 

r 

In  component  form,  this  is 

p  =  (p-y/sind 
q  =  \j/  sin  cp  cos  9  +  0  cos  cp 
r  =  >j/  cos  cp  cos  0-0  sin  cp 

Solving  these  equations  in  terms  of  p,  q  and  r,  results  in 

6  =  q  cos  (p-r  sin  (p 

y/  =  rsecdcos(p  +  qsec6sin(p 

<p=  p  +  rtandcoscp  +  qtandsirup 

Equivalently,  in  matrix  form; 


~<P~ 

0 

= 

V. 

1  tan  ft  in  (p  tan6fcos^> 

0  cos  (p  -sin  (p 

0  sec^in^»  sec6fcos9> 


(Eq.  3.19) 


(Eq.  3.20) 


(Eq.  3.21) 
(Eq.  3.22) 
(Eq.  3.23) 

(Eq.  3.24) 
(Eq.  3.25) 
(Eq.  3.26) 


(Eq.  3.27) 


The  equations  from  Eq.  3.21  through  Eq.  3.27,  also  known  as  the  gimbal  rate 
equations  and  are  quite  commonly  used  in  animation  and  simulation  [COOK92].  The 
matrix  in  Eq.  3.27  is  also  referred  to  as  the  T matrix  [FRAN69]. 
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3. 


Problems  with  Euler  Angles 


The  first  problem  results  in  divide-by-zero  errors  in  equations  involving 
trigonometric  functions.  Whenever  cos0  =  0  (0  =  ±7t/2) 


^  sin0  1 

tan  0  = - r  ,  sec  0  =  — 


cos0 


cos0 


(Eq.  3.28) 


are  undefined. 


A  second  problem  exists  when  pitch,  6 ,  goes  through  the  vertical; 


K 

*=±- 


(Eq.  3.29) 


At  that  point,  the  roll  and  azimuth  axes  become  coincidental.  This  is  called  gimbal  lock 
singularity.  “Gimbal  lock”  is  a  term  derived  from  a  mechanical  problem  that  arises  in  the 
gimbal  mechanism  used  to  support  a  compass  or  gyroscope.  The  final  rotation  matrix 
depends  on  the  order  of  multiplication.  It  is  sometimes  the  case  that  the  rotation  in  one 
axis  will  be  mapped  onto  another  rotation  axis.  Even  worse,  it  may  become  impossible  to 
rotate  an  object  in  a  desired  axis  [WATT98]. 

Both  these  problems  occur  because  Euler  angles  ignore  the  interaction  of  the 
rotations  about  separate  axes.  In  truth,  these  rotations  are  not  independent  of  each  other 
[WATT98], 

The  only  solution  to  going  through  a  vertical  orientation  is  to  fake;  i.e.,  fix  the 
code  so  a  division-by-zero  error  doesn’t  occur.  These  fixes  usually  prove  less  than 
satisfactory  and  make  it  impossible  to  track  an  object  through  all  possible  orientations 
[COOK92], 
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C.  QUATERNION  METHOD 

An  alternate  method  of  describing  orientation  that  has  been  gaining  popularity  in 
the  graphics  community  is  the  use  of  unit  quaternions.  It  is  not  a  new  method 
[COOK92]  ;  quaternions  have  been  around  for  more  than  150  years. 

The  quaternion  is  a  concept  related  to  three  dimensional  vectors,  but  which  allows 
the  representation  of  operations  such  as  rotations  not  directly  representable  with  vectors 
[PERV82].  A  quaternion  is  a  four  dimensional  vector  with  an  associated  quaternion 
product.  It  is  conventional  to  interpret  a  quaternion  as  a  generalization  of  a  complex 
number  with  a  real  number  part  and  a  vector  part. 

1.  Quaternion  Basics 

The  basis  for  quaternions  are  three  imaginary  “flags”  i,  j  and  k  where  [MCCA90] 


j2  =  k2  =  ijk  =  -1 

(Eq.  3.30) 

ij  =  -ji  =  k 

(Eq.  3.31) 

jk  =  -kj  =  i 

(Eq.  3.32) 

ki  =  -ik  =  j 

(Eq.  3.33) 

There  are  several  equivalent  ways  of  writing  quaternions  in  terms  of  their  four 
components;  one  way  is  the  Standard  Quadrinomial  Form  [PERV82]: 

Q  =  { a + fk  +  yj  +  Sk:  a,  fi,  y,S  real}  (Eq.  3.34) 

There  are  three  other  commonly  used  quaternion  notations. 

Linear  combination  of  four  components: 

q  =  w  +  xi  +  yi  +  zk  (Eq.  3.35) 
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Four  dimensional  vector: 


q  =  (wxyz )  (Eq.  3.36) 

Scalar  with  a  vector  “imaginary  part”: 

q  =  (w,v)  (Eq.  3.37) 

where  w  is  a  scalar  quantity  and  v  is  a  vector.  Quaternion  addition  is  defined  in 
the  same  manner  as  normal  vector  addition 

+42  =  ((wi +  w2)(xi  +x2)(yi  +y2)i^\  +^2))  (Eq-  3.38) 

If  s  is  a  scalar,  then  scalar  multiplication  with  a  quaternion  is  defined  as 

sq  =  (sw,sv)  (Eq.  3.39) 

Quaternion  multiplication  can  be  defined  using  Eq.  3.30  through  Eq.  3.33.  Let 
fit  =  Wj  +ix^  +  jy i  +kzj  and  q2  =  w2  +  ix2  +  jy2  +  kz2 •  Then 


4i  *42  =(wi w2  -xxx2-y\y2  -z\z2)  + 
i(xiw2  +  wix2  - z\y2  +  y\Z2)  + 
j(y\w2  +Z\x2  +wiy2  -xxz2)  + 
k(ziw2-yxx2  +x1y2  +  wiz2) 


(Eq.  3.40) 


Note  that  COOK92  has  sign  errors  in  quaternion  multiplication.  The  same  result  can  be 
accomplished  through  a  vector  dot  product,  vector  cross  product  and  vector  scalar 
multiplications  [MCGH98A]. 

4l*42  =  (vvjw2  -V!  •v2,w1v2  +  w2vi  +vj  xv2)  (Eq.  3.41) 


Because  of  the  vector  cross  product,  the  quaternion  product  is  not  commutative.  Some 
mathematical  facts  related  to  quaternions  are  as  follows  [SHOE94,  CRAI89] 

Quaternion  addition  rules; 

<71+42  =42+41  (Eq.  3.42) 


(4i  +42) +43  -4i  +(42  +43) 
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(Eq.  3.43) 


q+0= 0 +q 


(Eq.  3.44) 


q  +  (-q)  =  (-q)  +  q  =  0 

(Eq.  3.45) 

Quaternion  multiplication  rules; 

0?l42)#3  =  <7]  (#2^3) 

(Eq.  3.46) 

►5* 

11 

►— » 

II 

(Eq.  3.47) 

qq  1  =  q  lq  =  ] 

(Eq.  3.48) 

if  ^1^2  =  then  either  q j  =  0  or  q2  =0 

(Eq.  3.49) 

0?1  +  <?2  )#3  =  <? 1#3  +  #2*73 

(Eq.  3.50) 

The  quaternion  conjugate; 

q*  =  O ,-v)  =  (w,  -xi  -yj  -zk)~{w  -x  -y  -  z) 

(Eq.  3.51) 

The  quaternion  Norm; 

N(q)  =  qq*  =  w2  +|v|2  =w2  +  v»v  =  |q|2 

(Eq.  3.52) 

The  magnitude  of  a  quaternion  is  the  square  root  of  its  norm. 

M(q)  =  jN^) 

(Eq.  3.53) 

The  quaternion  inverse  is  evidently 

* 

H  A HO) 

(Eq.  3.54) 

and  for  a  unit  quaternion 

-1  * 
q  =  q 

(Eq.  3.55) 

which  is  easier  to  compute  than  the  inverse  of  a  matrix. 
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The  normalized  unit  quaternion  is  given  by 


qq  q 

Vnonwlked  ^  Jqq* 

Any  scalar  can  be  represented  as  a  quaternion. 

q  =  (5  0  0  0)  =  (5,0) 

Any  three  dimensional  vector  can  be  represented  as  a  quaternion. 

q  =  {Qxyz)  =  (0,v) 

As  an  alternative,  a  quaternion  can  be  represented  as 

q  =  w  +  v 

The  vector  part  of  quaternion  can  be  rewritten  as 

v  =  sr  =rs 

where, 


5  =  |v|  =  -J(*2  +y2  +z2) 


and  r  is  the  unit  vector  given  by 


v 


(Eq.  3.56) 


(Eq.  3.57) 

(Eq.  3.58) 

(Eq.  3.59) 

(Eq.  3.60) 

(Eq.  3.61) 


(Eq.  3.62) 


With  this  notation  a  quaternion  becomes 


q  =  w  +  rs 

Another  representation,  the  quaternion  exponential  form 

a  „w+rs  w  rs 
—  e  =  e  e 

ers  =  coss  +  r  sins 


(Eq.  3.63) 

is  defined  as  [MCGH98A]; 

(Eq.  3.64) 
(Eq.  3.65) 
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The  conjugate  becomes 


*  _ 
q  =w-rs 


(Eq.  3.66) 


2.  Rigid  Body  Rotation  with  Quaternions 

The  orientation  of  a  rigid  body  can  be  described  as  a  rotation  ( 6 )  about  a  single 
inclined  axis  (m).  Constraining  the  axis  (m)  to  be  of  unit  magnitude,  the  unit  quaternion 
(q)  representing  this  rotation  is  [MCGH97] 


fee 

q  =  Cos  —  ,uSin  — 

H  \  2  2. 


(Eq.  3.67) 


where  u  is  a  unit  vector  describing  the  axis  about  which  the  vector  p  is  to  be  rotated 
through  an  angle  e  [BACH97B].  The  rotation  of  a  vector,  p  by  a  quaternion,  q  is  defined 
as  [SHOE85,  FUND96] 

Protated  =  (Eq.  3.68) 

The  quaternion  rotation  defined  in  Eq.  3.68,  rotates  the  vector’s  perpendicular  component 
twice  the  angle  which  is  perpendicular  to  the  rotation  axis,  and  leaves  the  vector’s 
parallel  component  unchanged.  To  obtain  the  desired  rotation  half  of  the  rotation  angle  is 
used  in  the  construction  of  the  unit  quaternion  given  by  Eq.  3.67  [MCGH98A]. 

Every  rotation  has  two  representations  in  quaternion  space,  namely  q  and  -q,  with 
the  same  effect  [WATT98].  By  using  this  topological  oddity,  Eq.  3.68  can  be  rewritten 
as 

Protated  =  =  (-^)P(-?)_1  (Eq.  3.69) 

The  product  of  two  unit  quaternions  is  always  of  unit  magnitude.  The  product 

q2qi  produces  a  single  quaternion  describing  an  orientation  achieved  by  applying  q2 

relative  to  the  orientation  described  by  qj,  where  both  quaternions  are  expressed  in  Earth 
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coordinates.  If  rotations  are  described  by  quaternions  in  body  coordinates,  then  the  order 
of  the  product  is  reversed. 

It  should  be  noted  that,  unlike  Euler  angles,  quaternion  rotations  require  only  two 
trigonometric  functions  to  rotate  a  vector  and  experience  no  singularities  at  any  angle  of 
rotation. 

To  illustrate  quaternion  rotation  through  an  example,  let  p=(0  10  0)  describe  a 
body  oriented  along  the  x-axis,  wings  level,  headed  to  north.  A  positive  90°  rotation 
about  the  y-axis  can  be  represented  by 

q  =  (Cos45° , vSin45° )  (Eq.  3.70) 

=  (0.707  0  0.707  0)  (Eq.  3.71) 

where  v  =  (0  10). 

The  following  product  rotates  the  body  with  an  orientation  described  by  p  90° 
about  the  y-axis 

^rotated  =  4PQ~l  (Eq.  3.72) 

=  (0  0  0-1)  (Eq.  3.73) 

which  represents  an  orientation  along  the  -z-axis,  in  which  the  body  is  pointing  straight 
up  [BACH97B], 

3.  Derivation  of  Quaternion  Rates  from  Body  Rates 

Angular  rates  p,  q  and  r,  can  be  used  to  find  the  derivative  of  the  orientation,  q , 
relative  to  an  Earth  fixed  coordinate  system. 

For  small  0 


0 

2 


(Eq.  3.74) 
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Thus 


f  e  . 

-1~ 

(  O'] 

cos— ,vsm 

l.v- 

V  2 

2J  ~ 

v  2) 

(Eq.  3.75) 

Assuming  0  changes  linearly  with  time,  the  orientation  expressed  by  q  as  a 
function  of  time  becomes,  for  small  t: 


$(0  =  | 


(Eq.  3.76) 


The  vector  vO,  expresses  an  angular  rate  of  6  about  a  vector  v  in  body  coordinates.  Thus 

vQ  =  (p  q  r)  (Eq.  3.77) 

and  q(t)  becomes 


,  1  1  1 
0(0  =  1  h~pt  ~qt  -rt 


Taking  the  derivative  of  q(t)  with  respect  to  time  produces 


d  , .  .  ..  1  1  1  . 

-9(0  =  9  =  (0.-p-«-r) 


=  -(0pqr') 


(Eq.  3.78) 


,Ib 


CO 


(Eq.  3.79) 

(Eq.  3.80) 
(Eq.  3.81) 


If  qi  is  the  initial  orientation  in  Earth  coordinates  and  <72  is  a  second  rotation  in 
body  coordinates  then  <73  is  the  composite  of  the  two  rotations 

03  =  0102  (Eq- 3-82) 
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By  the  product  rule 


1 

<?3  =  q\q2  +W2  =  m2  =  2qi 8(0  (Eq-  3-83) 


In  general  [BACH97B] 

4  =  \qB*  =  ^ q{0p  q  r)  (Eq.  3.84) 

This  general  formula  can  be  used  with  Euler  integration  to  achieve  a  smooth 
rotation.  Eq.  3.84,  also  avoids  the  “branch  cut”  problem  of  the  Euler  angles.  After  every 
full  360  degree  rotation,  the  quaternion  representing  the  rotation  will  switch  to  its 
negative  [MCGH98A]  as  presented  in  Eq.  3.69. 

4.  Concatenating  Rotations 

Suppose  qi  and  q2  are  unit  quaternions  representing  two  rotations  relative  to  Earth 
coordinates,  and  it  is  desired  to  perform  qi  first  and  then  q2-  To  do  this,  we  apply  q2  to 
the  result  of  qj,  regroup  the  product  using  associativity,  and  find  that  the  composite 
rotation  is  represented  by  the  quaternion  q2  *  qi. 

q2  * (q i  *P*q1~1)*q2_1  =(92  *qi)*P*(qf1  *q2"1)  (Eq-  3.85) 

=  (q2  *qi)*P*(q2  *qi)-1  (Eq.  3.86) 

5.  Computing  Rotation  Matrices  from  Quaternions 

The  only  time  we  need  to  compute  a  matrix  is  when  we  want  to  transform  the 
object  using  a  homogenous  transformation.  Alternatively,  rotation  and  translation  can  be 
handled  separately,  eliminating  the  need  for  computing  the  rotation  matrix  [SKOP96]. 
Matrix  multiplication  requires  many  more  operations  than  a  quaternion  product.  Thus, 
efficiency  and  numerical  accuracy  is  improved  through  the  use  of  quaternions  rather  than 
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matrices.  A  rotation  matrix  for  the  given  rotation  can  be  calculated  from  a  quaternion 
representing  that  same  rotation.  The  general  rotation  matrix  is  as  follows; 


- matrix 


w2+x2_y  2_z2 

2xy  -  2wz 
2xz+  2wy 
0 


2xy  +  2wz 


2xz  -  2wy 


7799 
wz-x+y-z  2xy  +  2wx 


2yz-2wx 

0 


2  2  2.2 
w-x-y  +z 


0 
0 
0 

2  2  2  2 
w  +x+y  +z 


(Eq.  3.87) 


The  simplified  version  of  this  matrix  for  unit  quaternions  is  [SHOE85]; 


Q 


matrix 


l-2y2-2z2 

2xy  -  2wz 

2xz  +  2wy 
0 


2xy  +  2wz  2xz  -  2wy 
1  -  2x2  -  2 z2  2xy  +  2wx 
2yz  -  2wx  1  -  2x2  -  2 y2 


0 


0 


0 

0 

0 

1 


(Eq.  3.88) 


Using  this  matrix,  the  rotations  of  a  vector  P  can  be  computed  as  [COOK92] 

Protated  =  P*Qmatrix  (Eq.  3.89) 


6.  Slerp 

The  use  of  linear  interpolation  between  two  unit  quaternions  produces  non-unit 
quaternions  without  normalization.  Spherical  linear  interpolation  {slerp)  between  two 
unit  quaternions  is  a  natural  generalization  of  linear  interpolation  [SHOE85]  and  is 
obtained  by 


sin(l  -  u)Q 

slerp{qx  ,q2,u)  =  q] - — —  +  92 

sin  5.2 


sinwfi 
sin  £2 


(Eq.  3.90) 


qi«q2=c°s£2  (Eq.  3.91) 

where  u  is  between  0  and  1,  and  Q.  is  the  interpolation  angle  between  two  key 
quaternions.  For  the  opposite  direction,  the  interpolation  angle  becomes  2tz  -  Q 
[WATT98]. 
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Given  any  two  key  quaternions,  p  and  q,  there  exists  two  possible  arcs  along 
which  we  can  move.  One  of  these  arcs  is  shorter.  A  method  for  finding  the  shortest 
interpolation  between  the  quaternion  pairs  p  and  q,  and  p  and  -q  is  as  follows.  First  find 
the  magnitude  of  their  difference,  that  is  (p-q)*(p~q) ,  and  then  compare  this  to  the 
magnitude  of  the  difference  when  the  second  quaternion  is  negated,  that  is 
(p+q)  •  (p  +  q).  If  the  former  is  smaller,  then  we  are  already  moving  along  the  smaller 
arc  and  nothing  needs  to  be  done.  If,  however  the  second  is  smaller,  then  we  replace  q  by 
-q  and  proceed  [WATT98]. 

D.  COMPARISON  OF  EULER  ANGLES  AND  QUATERNIONS 

Quaternions  and  Euler  angles  each  have  their  own  advantages  and  disadvantages. 
The  most  significant  advantage  of  quaternions  is  that  no  singularity  exists  when  the 
elevation  angle  (0)  passes  through  ±idl.  Any  orientation  or  rotation  can  be  represented 
by  quaternions.  In  the  Euler  Method,  roll  and  azimuth  Euler  angle  rates  become 
undefined  due  to  division  by  zero.  Truncating  the  angles  at  ±xJ2  will  avoid  this  problem. 
However,  this  truncating  will  result  in  some  skipping  during  the  rotation  [COOK92]. 

Quaternion  rotation  avoids  the  “branch  cut”  problem  of  Euler  angles.  When  using 
quaternions  to  rotate  an  object,  after  every  full  rotation  the  quaternion  representing  the 
rotation  will  switch  to  its  negative. 

Quaternions  can  be  computed  directly  from  the  dynamic  equations,  bypassing  the 
computation  of  transcendental  functions  necessary  in  computing  Euler  angles.  This 
direct  arithmetic  operation  reduces  the  cost  of  computation  compared  to  matrix 
multiplication. 
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Quaternions  are  compact  and  simple.  However,  there  are  three  difficulties  with 
using  quaternions.  First,  each  orientation  of  an  object  can  actually  be  represented  by  two 
quaternions.  Since  rotation  about  the  axis,  v  by  an  angle  0  is  the  same  as  rotation  about 
-v  by  the  angle  -0;  the  corresponding  quaternions  are  antipodal  points  on  the  sphere  in 
4D.  Second,  orientations  and  rotations  are  not  exactly  the  same  thing.  In  an  animation, 
360°  rotation  is  very  different  from  a  rotation  of  0°.  With  the  first  one,  the  model  will  be 
animated  for  a  full  rotation.  In  the  second  one,  no  animation  will  be  executed.  After  the 
rotation  however,  the  same  quaternion  (1  0  0  0)  or  (-1  0  0  0)  represents  both.  Thus 
specifying  multiple  rotations  with  quaternions  requires  intermediate  control  points 
[FOLE97], 

The  third  difficulty  is  that  quaternions  provide  an  isotropic  method  for  rotation. 
The  interpolation  is  independent  of  everything  except  the  relation  between  the  initial  and 
final  rotations.  This  is  useful  for  interpolating  orientations  of  tumbling  bodies,  but  not  for 
interpolating  the  orientations  of  a  virtual  camera  in  a  scene.  Humans  strongly  prefer 
cameras  to  be  upright,  and  are  profoundly  disturbed  by  tilted  cameras  [FOLE97].  By  its 
very  nature,  the  notion  of  a  preferred  direction  cannot  easily  be  built  into  the  quaternion 
representation  [WATT98].  Quaternions  have  no  such  preferences,  and  therefore  should 
not  be  used  for  camera  interpolation.  The  lack  of  an  adequate  method  for  interpolating 
complex  camera  motion  has  led  to  many  computer  animations  having  static  cameras  or 
very  limited  camera  motion  [FOLE97]. 

E.  SUMMARY 

This  chapter  presented  a  brief  overview  of  two  methods,  namely  Euler  and 
Quaternion  methods,  used  to  represent  the  rotations  and  orientations  of  a  rigid  body.  This 
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chapter  also  provided  some  introductory  information  about  quaternion  operations.  The 
next  chapter  introduces  the  theory  and  the  mathematical  formulation  of  a  quaternion 
attitude  estimation  filter. 
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IV.  QUATERNION  ATTITUDE  ESTIMATION  FILTER 


A.  INTRODUCTION 

The  purpose  of  any  filter  is  to  separate  one  thing  from  another  [BROW97].  The 
main  purpose  of  the  Quaternion  Attitude  Estimation  Filter  is  to  combine  independent 
noisy  inertial  measurements  to  determine  the  orientation  of  a  tracked  object.  The 
Quaternion  Attitude  Estimation  Filter  uses  different  types  of  instruments  to  get  the 
measurement  values  and  implements  a  nonlinear  complimentary  filter.  Orientation 
estimates  are  corrected  by  minimizing  the  mean  square  measurement  error.  The 
Quaternion  Attitude  Estimation  Filter  is  designed  as  a  complimentary  filter.  However, 
with  the  addition  of  bias  estimation,  it  can  no  longer  be  considered  a  complimentary 
filter.  This  chapter  will  present  the  mathematical  theory  of  the  quaternion  filter,  the  filter 
linearization  theory,  bias  estimation  considerations  and  the  white  noise  effects  on  the 
filter. 

B.  QUATERNION  ATTITUDE  ESTIMATION  FILTER 

The  Quaternion  Attitude  Estimation  Filter  was  proposed  by  [MCGH96]  as  an 
alternative  representation  and  improvement  to  filters  based  on  Euler  Angles.  The 
Quaternion  Attitude  Estimation  Filter  is  designed  to  track  human  limb  segments  through 
all  orientations  as  part  of  an  inertial  tracking  system.  It  uses  three  different  types  of 
sensors  to  obtain  the  information  about  the  orientation  of  a  tracked  object.  These  sensors 
are  a  three-axis  accelerometer,  a  three-axis  angular  rate  sensor  and  a  three-axis 
magnetometer.  These  sensor  inputs  appear  in  Figure  4  as  (xyz),  ( pqr ),  and 
(b\  b2  bj) ,  respectively. 
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Magnetometer 


Figure  4:  Quaternion  Attitude  Estimation  Filter 
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Angular  Rate  Sensor 

(pqr) 


The  three-axis  accelerometer  measures  the  combination  of  forced  linear 
accelerations  and  the  reaction  force  due  to  gravity,  ameasured  =  a-  g  [FOXL94], 

The  three-axis  angular  rate  sensors  measure  angular  velocity.  The  angular  rate 
sensor  experiences  drift  errors  over  time.  To  correct  the  measured  angular  rate  sensor 
data  in  both  the  horizontal  and  vertical  planes,  accelerometer  and  magnetometer 
information  is  required. 

Normally,  the  direction  of  the  Earth’s  gravity  vector,  m,  expressed  as  a  unit 
vector,  is  down.  For  low  frequency  considerations,  the  linear  accelerations  of  a  body 
average  to  zero.  That  is,  in  the  long  term,  the  accelerometer  senses  only  those 
accelerations  due  to  gravity  [BACF197B].  Thus,  on  the  average,  the  three-axis 
accelerometer  returns  the  gravity  vector  or  the  local  vertical,  ameasured  =  -g  [HENA97]. 
The  down  unit  vector  in  quaternion  form  can  be  represented  as 

m  =  [0  0  0 1]  (Eq.  4.1) 

The  three- axis  magnetometer  measures  the  Earth’s  magnetic  field,  b,  in  body 
coordinates  [HENA97].  The  main  purpose  of  the  magnetometer  is  to  sense  the  drift  error 
of  the  angular  rate  sensor  about  the  vertical  axis.  This  can  not  be  sensed  by  the 
accelerometer  [HENA97].  The  Earth’s  unit  magnetic  field,  n,  at  any  location  is  known 
and  can  be  calculated  or  looked  up  [BACH97B].  The  Earth’s  unit  magnetic  field  can  be 
represented  as  a  unit  vector  by; 

n  =  [0  «2  n3 1  (Eq.  4.2) 

The  characteristics  of  the  Earth's  magnetic  field  in  the  local  area  must  be 
determined  to  find  n,  which  incorporates  the  declination  and  the  dip  angle.  Magnetic 
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declination  is  defined  as  the  difference  between  the  north  compass  heading  and  the  true 
geographic  north  at  a  given  location  on  the  Earth’s  surface.  Monterey,  CA  requires  a 
correction  of  15°.  The  lines  of  the  Earth's  magnetic  field  are  parallel  to  the  surface  at  the 
equator.  However,  as  one  approaches  the  magnetic  poles,  they  become  increasingly 
vertical.  Dip  angle  is  the  correction  of  the  measure  for  the  downward  deflection  of  the 
local  magnetic  field.  In  Monterey,  a  correction  of  -60°  is  applied  [HENA97]. 

C.  DERIVATION  OF  THE  FILTER 

The  Quaternion  Attitude  Estimation  Filter  takes  normalized  measurements  from 
the  three-axis  accelerometer  and  from  the  three-axis  magnetometer  as  shown  in  Figure  5 
[HENA97], 

Magnetometer 

(i>l  b2  b3) 

J 

AWiW7 


Figure  5:  Upper  Left  Part  of  Quaternion  Attitude  Estimation  Filter 

The  accelerometer  returns  the  local  vertical,  h  and  the  magnetometer  returns  the 

Earth’s  local  magnetic  field,  b.  This  information  is  expressed  in  body  coordinates  as 
3-dimensional  unit  vectors  in  the  form  of  the  pure  imaginary  unit  quaternions  given  in 
Eq.  4.3  and  Eq.  4.4. 

h  =  [0hlh2h3]  (Eq.  4.3) 


Accelerometer 

(xyz)  =  (h1h2h3) 
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b  =  [0  £>j  &2  b3  ] 


(Eq.  4.4) 


The  filter  combines  these  vectors  to  get  the  measured  vector,  jq  given  by; 


h 

h2 


yo  = 


h 

h 

b2 


h 


6x1 


(Eq.  4.5) 


After  forming  the  measured  vector,  the  filter  calculates  the  computed 
measurement  vector,  y{q) ,  using  the  estimated  orientation  quaternion,  .  The  Earth’s 
unit  gravity  vector,  m,  and  the  local  magnetic  field  vector,  n,  are  rotated  into  body 
coordinates  in  order  to  permit  comparison  with  the  measured  orientation  vector.  The 

A  A 

estimated  orientation  of  body,  q ,  determines  h  and  b  by; 


h  =  q  1 mq  (Eq.  4.6) 

and 

b  =  q~lnq  (Eq.  4.7) 

Where  the  real  or  scalar  part,  w,  is  always  zero.  The  estimated  orientation 
quaternion  is  set  when  the  system  is  initialized.  The  computed  measurement  vector  is 
given  by; 

y{q)  =  {q  mq,q  nq) 


(Eq.  4.8) 


By  removing  the  w  values  of  h  and  b ,  the  computed  measurement  vector  can  be 


constructed  as; 


(Eq.  4.9) 


The  error  is  computed  by  taking  the  difference  between  the  measured  vector  and 


the  computed  vector  and  is  given  by 

*($)6jc1  =  y($)6x\  ~  5^06jc1  (Ecl-  4- 10) 


or 

'k-h ' 

h2-h2 

hi  -  hi. 

£(q)=  /  (Eq.  4.1 1) 

b\  ~b\ 

b2  —b2 

A  -*3  J^l 

To  correct  this  error  an  iterative  method  must  be  applied.  In  [HENA97],  two 
different  iteration  methods  were  tested  in  simulation  namely;  Gradient  Descent  and 
Gauss-Newton.  The  Gradient  descent  method  linearly  converged,  but  needed  dozens  of 
steps  to  arrive  at  a  satisfactory  result.  The  Gauss-Newton  method  converged 
quadratically  and  needed  less  iteration.  Based  on  this  result,  Gauss-Newton  was  used  for 
this  thesis. 
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For  the  Quaternion  Attitude  Estimation  Filter  the  error  correction  vector  which 
approximates  Gauss-Newton’s  iteration  formula  [MCGH63]  is  given  in  Eq.  4.12; 


=4[X7X]'‘V 


(Eq.  4.12) 


where  the  X  matrix  is  given  by 


yT.  — 
Ay  ~ 


dyt 


(Eq.  4.13) 


4x6 


Appendix  A  presents  the  derivation  of  the  X  matrix  given  in  Eq.  4. 13. 
The  least-squares  criterion  function  is  given  by 


00?)  lxi  =£(§)'  1x6  £(£>6x1 


(Eq.  4.14) 


where 


e{qf  =  [(*1  -hx){h2 -h2)(h3  -h3){bx-bx){b2 -b2)(b3 -h)]lx6  (Eq-  4-15) 

Substituting  Eq.  4.15  into  Eq.  4.14  yields 

00?)  1x1  =[(*1  -h)2  +  (4 -h2)2 +(h3-h3)2  +(4  -bx)2  +  (4  -  b2 )2  +  (4  -&3)2](Eq.  4.16) 


The  gradient  of  the  error  criterion  function  is  defined  as; 

\T 


f 


V<p{q)  = 


dtp  dtp  dtp  dtp 
{dq0  dqx  dq2  dq3) 


■2X  4x6^77/)  6x1 


(Eq.  4.17) 


The  derivation  of  this  gradient  is  given  in  Appendix  B. 
Substituting  Eq.  4.17  into  Eq.  4.12  yields 

=~[xTx\'2XTe(q) 


(Eq.  4.18) 


=  -Xrx]‘‘4X4V(4)6*l 


(Eq.  4.19) 
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The  q£  in  Figure  4  represents  the  output  from  the  top-half  of  the  filter,  and  it  will 
be  used  to  correct  the  computed  rate  quaternion  derivation,  q .  To  find  the  output  of  the 
top-half  of  the  filter  the  error  correction  vector,  Ag,  must  be  multiplied  by  a  constant 
gain,  k 

qe  =  kAq  (Eq.  4.20) 

A  scalar  multiplier,  a ,  is  used  when  dealing  with  data  corrupted  by  noise 

a  =  kAt  (Eq.  4.21) 

1.  Linearization 

Aq fun  is  the  correction  to  q  based  upon  the  Gauss-Newton  iteration  formula. 
Assuming  there  is  no  measurement  noise,  and  that  the  computation  of  A q fuu  is  exact,  it 
follows  that  [MCGH98C] 

A$4xl  =4true-4  (Eq.  4.22) 

A<lfull=<ltrue-q  (Eq.  4.23) 

The  signal  flow  diagram  (SFG)  representing  this  linearized  system  is  presented  in 
Figure  6. 
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Applying  Mason’s  formula  [KU095]  to  Figure  6  to  obtain  the  transfer  function 
produces 


q  kpJ_+^J_  p  *(1  +  kp  *) 

Qtrue  1  +  kp  1  1  +  kp  ^ 


(Eq.  4.24) 


thus 


Q  =  P  ^4  true  =  Vtrue  (Eq.  4-25) 

This  result  shows  that  regardless  of  the  value  of  k,  if  nj  and  n2  are  zero,  then 
q  =  qtme  ■  This  is  as  expected  for  a  complimentary  filter. 

2.  Response  to  Initial  Condition  Error 

Suppose  the  sensors  are  static  and  q  is  not  correctly  initialized  then 

W=(1000)  (Eq.  4.26) 

^0  =  (l9^y^)  (Eq.  4.27) 

If  the  noise  sources  and  n2  are  both  zero  then  the  SFG  can  be  redrawn  as  in  Figure  7 


<7  true  A7  full  ^  7 


Figure  7:  SFG  for  Initial  Condition  Error  Analysis 

From  Eq.  4.23 

A<2>//=( 0  -dx  -dy  -dz)  (Eq.  4.28) 


Since  the  first  component  did  not  change,  q  will  have  the  form 


q  =  (\xyz) 


(Eq.  4.29) 
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Using  Eq.  4.27,  the  transform  domain  SFG  for  the  scalar  x  is  as  follows 


a. 


Figure  8:  Transform  Domain  SFG  for  x(s) 

From  this  SFG,  and  application  of  Mason’s  formula  it  follows  that  [MCGH98B] 


x(  s)  s  1  1 

dx  1  +  fcT1  s+k 


(Eq.  4.30) 


and  therefore 


x(t)  =  dxe~kt  (Eq.  4.31) 

Equivalent  results  apply  for  y(t)  and  z(t) .  Thus,  any  transient  errors  in  q 
resulting  from  erroneous  input  from  the  magnetometer  and  the  accelerometer  will  persist 
for  a  time  inversely  proportional  to  k.  Specifically  if 

*Aq  =  j  (Eq.  4.32) 

then  for  any  disturbance  dx ,  the  resulting  error  in  the  x  component  of  q  will  be 


-t 

z-(t)  =  dxeX^  (Eq.  4.33) 

This  error  will  be  reduced  to  37%  of  the  initial  value  by  time  t  =  x^q .  This 
shows  that  the  filter  must  use  a  big  k  value.  On  the  other  hand,  if  the  maneuver  time 
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constant  is  'CmflneHver ,  it  is  required  that  xAq  be  much  larger  than  this  value  to  suppress 
maneuver  noise.  Therefore,  the  qualitative  requirement  becomes 

- - - »  k  »  (Eq.  4.34) 

^ maneuver  ^ bias 

and  the  constraint  for  At  [MCGH98C]  is 

A t<\  (Eq.  4.35) 

k 

Reasonable  values  for  At  and  k  can  be  further  adjusted  by  experimental  means. 

3.  Rate  Sensor  Bias  Correction 

Drift  is  the  tendency  of  sensor  output  to  change  over  time  with  no  change  in 
sensor  input  and  causes  relentlessly  increasing  orientation  measurement  errors.  This 
error  is  sometimes  termed  a  bias.  The  integration  of  a  bias-ridden  angular  rate  signal  will 
cause  a  steady  build-up  of  error  over  time.  This  leads  to  an  incorrect  estimation  of  the 
body  orientation  relative  to  the  Earth-fixed  coordinate  system.  Angular  rate  sensor  biases 
typically  change  unpredictably  over  time,  making  a  simple,  complete  compensation 
impossible  [ROBE97]. 

The  filter  takes  the  angular  rate  sensor  readings  and  computes  the  bias-corrected 
output,  Bm .  To  find  the  bias-corrected  output,  the  filter  must  estimate  the  bias  error  and 
use  this  estimate  to  correct  subsequent  measurements.  Discrete  low  pass  filter  theory 
provides  a  method  for  obtaining  a  rate  bias  estimate  [KU095]. 

The  quaternion  filter  bias  estimation  is  based  on  Shallow  Water  AUV  Navigation 
System  (SANS)  experience  [ROBE97].  The  approach  taken  to  deal  with  this  problem  in 
SANS  was  to  estimate  an  initial  value  for  the  bias  by  averaging  the  rate  sensor  output 
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before  beginning  maneuvering  and  then  tracking  the  time-varying  drift  with  a  very  long 
time  constant  low  pass  filter  [MCGH98D].  The  signal  flow  graph  for  4 measured  is  given 
in  Figure  9. 


Qtrue  P.  measured 


Figure  9:  Bias  Estimation  Filter 

Evidently 


^measured 
4  true 


1  +  k  biasP  P+kbias 


(Eq.  4.36) 


Thus,  with  the  addition  of  bias  estimation,  the  quaternion  filter  is  no  longer  a 
complimentary  filter  since  constant  rates  of  rotation  are  eliminated.  This  effect  can  be 
minimized  by  applying  the  constraint 

k  »  kbias  (Eq-  4-37) 

If  k  is  too  large,  then  the  filter  may  become  unstable  or  too  much  maneuver  induced  error 
may  appear  in  q  [MCGH98D]. 

The  computed  quaternion  derivative,  4 ,  is  computed  by  the  lower  left-hand 
portion  of  the  filter.  Figure  10  [HENA97].  The  expression  for  <7  is  as  given  in  Eq.  3.84. 


.  1  „  B 

q=2q  03 


(Eq.  4.38) 
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Estimated  Bias 


<j>b  %  %) 


Figure  10:  Lower  Left  Part  of  Quaternion  Attitude  Estimation  Filter 

The  bias-corrected  angular  rate  quaternion  is  corrected  for  the  estimated  bias  (pb 

qb  rb).  The  corrected  quaternion  derivative  q  is  computed  by 

q  =  q~q£  (Eq.  4.39) 

The  corrected  quaternion  derivative  is  then  numerically  integrated  and 
normalized.  The  resulting  numerical  integration  equation  becomes 

qn+l  =qn+^qnBcoAt  +  k[xTx]~lXT£(qn)At  (Eq.  4.40) 

The  normalized  result  is  the  estimated  orientation  quaternion,  q ,  which  yields  the 
next  approximation.  This  approximated  orientation  quaternion  can  be  used  for  the 
graphical  representations  of  a  tracked  object  in  a  virtual  environment. 

D.  WHITE  NOISE  EFFECTS 

White  noise  can  be  represented  by  a  squence  of  statiscally  independent  numbers. 
The  white  noise  effects  on  the  filter  for  the  Gauss-Newton  method  were  tested  by  using  a 
simulation  program  written  in  ANSI  Common  Lisp  [HENA97],  A  6D  white  noise  vector 
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was  added  to  the  measurement  vector  and  a  different  white  noise  sample  was  used  for 
every  iteration.  A  scalar  multiplier,  a ,  was  used  to  decrease  the  white  noise  effects  on 
the  filter.  The  modified  filter  with  the  white  noise  is  shown  in  Figure  1 1 . 


Magnetometer  White  Noise 


Figure  11:  Modified  Filter  with  White  Noise 

This  modification  is  only  made  for  testing  the  white  noise  effects  on  the  filter;  in 

real-time  implementations  this  white  noise  must  assumed  to  be  in  the  measurements  from 
the  sensors.  The  filter  responded  to  the  simulated  white  noise  in  a  corrective  manner  and 
the  results  were  very  close  to  the  results  with  perfect  noisless  inputs. 

Two  important  results  were  obtained  from  this  test.  The  first  one  was  that 
reducing  a  improves  results  in  the  long  term,  but  takes  longer  to  converge.  The  second 
result  was  that  for  tracking  motion,  the  optimal  a  depends  on  the  rate  of  motion  and  the 
noise  level.  The  simulation  results  without  white  noise  and  with  white  noise  are 
presented  in  Figure  12  and  Figure  13  respectively.  The  Figure  12  shows  an  exponential 
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Figure  13: 10  Degree  Offset,  a=0.1,  At=0.1,  With  Gaussian  White  Noise 
Noise  Standard  Deviation=0.57  Degree 


convergence.  This  result  proves  the  value  of  the  linearization  theory  and  the  error 
convergence  formula  given  in  Eq.  4.33. 

E.  SUMMARY 

The  Quaternion  Attitude  Estimation  Filter  offers  significant  improvements  over 
filters  using  Euler  angles.  Inputs  are  obtained  from  a  3-axis  accelerometer,  a  3-axis 
angular  rate  sensor  and  a  3-axis  magnetometer.  No  singularities  or  divide  by  zero  errors 
exist  for  any  orientation  and  no  trigonometric  functions  are  required.  Using  unit 
quaternions  makes  calculating  inverses  simple  and  provides  an  increase  in  computational 
efficiency  over  matrix  multiplication. 

The  next  chapter  presents  the  hardware  configuration  of  the  quaternion  attitude 
estimation  filter  system. 
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V.  SYSTEM  HARDWARE  CONFIGURATION 

A.  INTRODUCTION 

The  hardware  components  of  the  quaternion  attitude  estimation  filter  system  are 
configured  for  testing  purposes.  These  components  consist  of  a  sensor  block  which  holds 
a  3-axis  magnetometer,  a  3-axis  accelerometer  and  a  3-axis  rate  sensor,  a  12  VDC  power 
source,  an  I/O  connector,  a  16-bit  analog  to  digital  (A/D)  converter  and  a  PC,  Figure  14. 

I/O  Connector 


Figure  14:  System  Hardware  Configuration 

The  power  source  supplies  12  VDC  power  for  the  sensor  block.  The  ribbon 

cables,  that  carry  actual  output  voltages  from  the  sensors,  are  connected  to  the  separate 
analog  input  channels  on  the  I/O  connector  via  a  breakout  header.  The  A/D  converter 
card  is  physically  in  the  PC  and  receives  data  from  the  sensors  via  the  I/O  connector. 
This  chapter  will  summarize  the  hardware  configuration  and  the  main  hardware 
component  capabilities  used  in  the  quaternion  attitude  estimation  filter  system. 
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B.  HARDWARE  DESCRIPTION 


1.  Sensor  Block 

The  sensor  block  contains  three  individual  sensors.  The  main  purpose  of  this 
sensor  block  is  to  implement  a  small  9-axis  sensor  system.  The  sensor  block  contains  a 
3-axis  magnetometer,  a  3-axis  accelerometer,  and  a  3-axis  rate  sensor.  Figure  15.  This 
sensor  block  was  constructed  by  [MCKI98]. 


Figure  15:  The  Sensor  Block  [MCKI98] 


The  sensor  block  is  covered  with  foam.  The  foam  covering  provides  shock 
protection  and  a  stable  temperature  environment  for  the  sensors.  The  measurements  of 
this  package  with  foam  coverage  are  10.7  x  5  x  3.7  cm,  and  without  foam  coverage  are 
9.1  x  3.9  x  2  cm.  The  size  of  the  sensor  block  is  very  small,  but  with  smaller  sensors,  a 
smaller  sensor  block  can  be  constructed. 

The  outputs  from  this  sensor  block  are  connected  to  the  I/O  connector  by  a  ribbon 
cable  via  a  breakout  header.  The  DC  power  supply  is  also  connected  to  this  breakout 
header. 

The  3-axis  magnetometer  is  a  Honeywell  HMC2003  type  magnetic  sensor,  Figure 
16.  The  HMC2003  is  a  very  small  hybrid  chip  and  can  detect  magnetic  fields  of  less  than 
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40  microgauss  about  three  axes.  Some  characteristics  of  HMC2003  are  given  in  Table  1 
[HONE98], 


Figure  16:  Honeywell  HMC2003  3-axis  Magnetic  Sensor  Hybird  [HONE98] 


Characteristic 

Range 

Units 

Supply  Voltage 

6-15 

VDC 

Field  Range 

-2-2 

gauss 

Output  Voltage 

0.5 -4.5 

V 

Resolution 

40 

[i  gauss 

Bandwidth 

1 

KHz 

Null  Field  Output 

2.3 -2.7 

V 

Table  1:  3-axis  Magnetometer  Specifications 


The  3-axis  accelerometer  is  a  Crossbow  Technology  Inc.  CXL04M3  type  3-axis 
accelerometer,  Figure  17.  The  CXL04M3  can  measure  accelerations  about  three  axes. 
Characteristics  of  CXL04M3  are  given  in  Table  2  [CROS98]. 


Figure  17:  Crossbow  CXL04M3  3-axis  Accelerometer  [CROS98] 
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Characteristic 

Range 

Units 

Supply  Voltage 

+  5 

VDC 

Span  Range 

±4  ±  5% 

G 

Output  Voltage 

0-5 

V 

Resolution 

5 

mGrms 

Bandwidth 

DC- 100  ±  5% 

Hz 

Sensitivity 

500  ±  5% 

mV/G 

Zero  G  Output 

2.5  ±0.1 

V 

Table  2:  3-axis  Accelerometer  Specifications 


The  3-axis  rate  sensor  is  a  Tokin  America  Inc.  CG16D0  type  solid  state  rate 
sensor.  The  CG16D0  can  measure  the  angular  velocities  about  three  axes.  Some 
characteristics  of  CG16D0  are  given  in  Table  3  [TOKI98]. 


Characteristic 

Range 

Units 

Supply  Voltage 

+  5 

VDC 

Detect  Range 

±90 

deg/sec 

Output  Voltage 

0-5 

V 

Bandwidth 

100 

Hz 

Sensitivity 

1.1  ±20% 

mV/deg/sec 

Reference  Voltage  Output 

2.4  ±  10% 

V 

Table  3:  3-axis  Angular  Rate  Sensor  Specifications 

The  sensor  block  can  support  100  Hz  sampling  rates.  The  magnetometer  has  a 
1  KHz  bandwidth. 
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2. 


A/D  Converter 


The  A/D  converter  is  a  National  Instruments  Corporation®  [NATI98]  PCI-MIO- 
16XE-50  data  acquisition  (DAQ)  card,  Figure  18.  It  is  physically  inserted  into  the  PCI 
slot  of  a  PC  motherboard.  The  PCI-MIO-16XE-50  is  a  16-bit  A/D  converter,  it  has  16 
single-ended  or  8  double-ended  analog  input  channels.  Its  maximum  sampling  rate  is 
20  K  samples/sec.  The  recommended  warm  up  time  is  15  minutes. 


Figure  18:  National  Ins.  PCI-MIO-16XE-50  Data  Acquisition  Card  [NATI98] 

It  can  measure  analog  input  voltages  between  0V  -  10V  (single  sided)  and  ±10V 

(double  sided).  Sensor  output  voltage  connections  with  A/D  converter  channels  are 
shown  in  Table  4. 


Angul 

iar  Rate  Sensor 

Accelerometer 

Magnetometer  | 

X 

y 

z 

X 

y 

z 

X 

y 

z 

p 

T 

r 

hi 

h2 

h3 

bl 

b2 

b3 

Channel  No 

0 

1 

2 

3 

4 

5 

6 

7 

8 

Pin  No 

68 

33 

65 

30 

28 

60 

25 

57 

34 

Ground 

67 

29 

64  | 

Table  4:  Sensor  Channel  Connections 


3.  Other  Components 

The  computer  used  in  the  research  described  in  this  thesis  contains  a  333  Mhz 
Intel  Pentium  II  CPU  and  64  MB  of  RAM.  The  operating  system  is  Microsoft  Windows 
95™. 
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Figure  19:  National  Ins.  SCB68  I/O  Connection  Board  [NATI98] 

The  I/O  connector  is  a  National  Instruments  Corporation®  [NATI98]  SCB68  type 

I/O  connection  board,  Figure  19.  This  I/O  connector  connects  the  output  ribbon  cable 
from  the  breakout  header  with  the  associated  channels  of  the  A/D  converter. 

C.  SUMMARY 

The  hardware  configuration  described  in  this  chapter  was  used  for  the  quaternion 
attitude  estimation  filter  system.  For  further  information  about  the  hardware  components 
refer  to  the  related  product  catalogs  and  the  web  sites  of  the  manufacturers.  The 
estimated  equipment  and  material  cost  of  the  hardware  except  the  PC  is  about  $3,500. 
The  next  chapter  presents  the  software  design  documentation  of  the  quaternion  attitude 


estimation  filter. 


VI.  SOFTWARE  DEVELOPMENT 


A.  INTRODUCTION 

The  Unified  Modeling  Language  (UML)  is  a  language  for  expressing  the 
constructs  and  relationships  of  complex  systems.  A  critical  aspect  of  real-time  systems  is 
how  time  itself  is  handled.  The  design  of  a  real-time  system  must  identify  the  timing 
requirements  of  the  system  and  ensure  that  the  system  performance  is  both  correct  and 
timely  [DOUG98].  UML  is  particularly  well  suited  for  designing  real-time  embedded 
systems. 

The  features,  concepts  and  visual  notations  used  in  UML  are  explained  in 
[DOUG98].  During  the  software  development  process  of  the  quaternion  attitude 
estimation  filter  the  Rational  Rose  4.0  software  design  tool  [RATI98]  was  used  to 
implement  the  system  in  UML. 

The  steps  in  software  development  are  system  requirements  analysis,  system 
analysis,  system  design  and  implementation  [DOUG98].  This  chapter  will  present  these 
steps  for  the  quaternion  attitude  estimation  filter  software  development. 

B.  SYSTEM  REQUIREMENTS  ANALYSIS 

The  quaternion  attitude  estimation  filter  (QAEF)  is  a  complementary  filter 
designed  to  track  human  limb  segments  through  all  orientations  as  part  of  an  inertial 
tracking  system  The  QAEF  uses  three  different  sensors  to  obtain  the  information  about 
the  orientations  of  a  tracked  object.  These  sensors  are  a  3-axis  accelerometer,  a  3-axis 
angular  rate  sensor  and  a  3-axis  magnetometer.  The  system  gets  these  sensor  readings 
through  an  I/O  connector  by  using  an  analog  to  digital  (A/D)  converter.  The  system 
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samples  and  filters  these  data  to  produce  an  estimated  orientation  of  the  tracked  object  in 
the  quaternion  form. 

The  QAEF  will  filter  the  data  from  only  one  set  of  sensors,  but  is  intended  to  be 
the  building  block  for  a  body  suit  using  several  sensor  blocks.  The  system  use  case 
diagram  is  presented  in  Figure  20. 


Figure  20:  Use  Case  Diagram 


Figure  21:  Context  Diagram 

The  system’s  interaction  with  the  actor  objects  is  shown  in  the  context  diagram. 
Figure  21.  A  sequence  diagram  which  shows  the  sequence  of  messages  between  objects 
is  presented  in  Figure  22.  The  scenario  diagram  is  the  same  as  the  sequence  diagram, 
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because  the  system  has  a  continuous  behavior  and  there  are  no  external  events  related  to 
the  system. 


S  Block  :  Sensors 


AD  Converter :  Converter 

mvSamoler : 

Mv  Filter :  Filter 

Quat  Filter  :  Qaef 

Sampler 

1 :  Get  data 


□ 


i 

2:  Update  buffer 

t 

1 

1 

I 

1 

1 

1 

<  1 

1 

1 

1 

3:  Get  new  sample  , 

1  [ 

!  4:  Request  new  data 

j 

L  L 

'  5:  Transfer  data  7 


*  6:  Sample  data 


7:  Correct  bias  error 


,  8:  Filter  data 


i  9:  Update  result 


1] 


AUV :  Simulation 
Program 


10:  Get  result 


Figure  22:  Sequence  Diagram 
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The  system  will  target  a  sampling  rate  between  40  Hz  and  100  Hz,  and  an  update 
rate  of  at  least  30  Hz  for  real-time  applications.  The  use  cases  and  the  system  goals  are 
presented  in  Table  5  and  Table  6  respectively. 


Goal  1 

System  has  to  get  data  from  the  sensors  and  convert  it  into  digital 
data  with  a  sampling  rate  of  at  least  40  Hz 

Goal  2 

System  has  to  have  at  least  a  30  Hz  update  rate  at  the  end  of  the 
filtering  process. 

Table  5:  System  Goals 


Use  Case 

(Ul)  Get  raw  data 

Actor 

Sensors 

Goal 

Traced  to  Goal  1 

Preconditions 

Sensors  and  A/D  Converter  are  running 

Description 

Object  rotates  or  keeps  its  rotation.  System  will  always  get  the 
readings  from  the  sensors,  convert  this  data  into  digital  data 

Sub  Use  Case 

None 

IfSSSSSSSHI 

None 

Activities 

Get  the  readings  and  write  into  a  buffer 

Postconditions 

Continue  getting  data 

Use  Case 

(U2)  Filter  data 

Actor 

Simulator  program 

Goal 

Traced  to  Goal  2 

Preconditions 

Sensors,  A/D  Converter  and  system  are  running 

Description 

System  will  get  the  data  from  buffer  and  produce  an  estimated 
orientation  in  the  quaternion  form  by  filtering  the  data 

Sub  Use  Case 

None 

None 

Activities 

Read  data  from  the  buffer,  apply  filtering  algorithms  and  calculate  an 
estimated  orientation 

Postconditions 

Continue  filtering  processes 

Table  6:  System  Use  Cases 


The  major  functions  performed  by  the  QAEF  are  data  acquisition,  data  filtering, 
and  updating,  displaying  and  sending  the  estimated  results.  The  actual  system  functions 
are  given  in  Table  7. 
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Svstem  Functions 

Rank 

Use  case 

Rl.  Data  Acquisition  Functions 

Rl.l  Get  sensor  readings 

Essential 

U1 

R1.2  Write  data  into  the  buffer 

Essential 

U1 

R1.3  Read  data  from  the  buffer 

Essential 

U1.U2 

R2.  Data  Filtering  Functions 

R2. 1  Average  the  data 

Essential 

U2 

R2.2  Calculate  measurement  vector 

Essential 

U2 

R2.3  Calculate  computed  measurement  vector 

Essential 

U2 

R2.4  Calculate  error 

Essential 

U2 

R2.5  Calculate  bias  error 

Essential 

U2 

R2.6  Calculate  X-matrix 

Essential 

U2 

R2.7  Calculate  Gauss-Newton  iteration 

Essential 

U2 

R2.8  Calculate  numerical  integration 

Essential 

U2 

R2.9  Convert  data  into  digital 

Essential 

U2 

R3.  Matrix  Operations 

R3.1  Multiply 

Essential 

U2 

R3.2  Invert 

Essential 

U2 

R3.3  Transpose 

Essential 

U2 

R3.4  Convert  to  quaternion 

Essential 

U2 

R3.5  Print 

Essential 

U2 

R4.  Quaternion  Operations 

R4.1  Add 

Essential 

U2 

R4.2  Subtract 

Essential 

U2 

Table  7:  System  Functions 
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R4.3  Quaternion  multiplication 

Essential 

U2 

R4.4  Rotation 

Essential 

U2 

R4.5  Dot  product 

Essential 

U2 

R4.6  Derivative 

Essential 

U2 

R4.7  Normalize 

Essential 

U2 

R4.8  Print 

Essential 

U2 

R5.  Output  Functions 

R5.1  Print  to  the  screen 

Desired 

U2 

R5.2  Write  into  a  file 

Optional 

U2 

R5.3  Write  into  a  buffer 

Optional 

U2 

Table  7:  System  Functions  (Cont’d) 


C.  SYSTEM  ANALYSIS 

The  QAEF  system  boundary  is  limited  by  the  A/D  converter.  The  actual  filter 
application  will  pass  the  estimated  results  to  a  3D  simulation  or  to  a  tracking  program. 
For  this  research  the  system  will  print  the  results  to  the  screen  and  will  write  them  into  a 
file.  The  system  behavior  is  continuous  and  will  implement  a  Proportional  Integral 
Derivative  (PID)  control  loop.  The  system  will  run  on  a  PC  under  the  Microsoft 
Windows  95™  operating  system.  The  identified  objects  are  shown  in  the  object  diagram. 
Figure  23.  The  system  state  chart  showing  the  general  system  behavior  is  in  presented  in 
Figure  24. 
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Figure  23:  Object  Diagram 


Figure  24:  System  Statechart 

The  system  functions  are  distributed  to  the  identified  classes.  The  associations 
and  the  relations  of  these  classes  are  determined  from  the  object  diagram  and  from  the 
sequence  diagram  to  create  the  class  diagram  for  the  system.  The  class  diagram  is 
presented  in  Figure  25. 
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QAEF 


D.  SYSTEM  DESIGN 


Design  details  the  largest  scale  software  structures,  such  as  subsystems,  packages 
and  tasks,  and  specifies  the  internal  primitive  data  structures  and  algorithms  within 
individual  classes  [DOUG98].  The  QAEF  system  hardware  configuration  is  explained  in 
Chapter  V.  The  deployment  diagram  for  the  QAEF  hardware  is  presented  in  Figure  26. 

The  tasks  are  identified  as  “Get  Sensor  Readings”,  “Sample  Data”  and  “Filter 
Data”.  The  “Get  Sensor  Readings”  task  gets  sensor  readings  and  writes  these  readings 
into  the  double  buffer.  The  Filter  Data  task  calls  its  server  task,  the  Sample  Data  task,  to 
get  the  current  sample  readings.  The  Filter  Data  task  receives  these  samples  via  an  array 
and  then  filters  the  data  to  produce  an  estimated  output  result.  Sample  Data  task  sends  a 
data  transfer  request  signal  to  the  Get  Sensor  Readings  task,  and  then  the  Get  Sensor 
Readings  task  updates  the  transfer  buffer  with  half  of  the  double  buffer.  The  Filter  Data 
task  should  wait  for  buffer  updating  and  the  sampling  data  processes.  The  data 
transferring  process  from  the  double  buffer  to  the  transfer  buffer  will  have  a  timeout. 
During  this  period,  the  Filter  Data  task  will  be  waiting  for  the  sampled  data  and  for  the 
Get  Sensor  Readings  task  to  finish  this  transfer.  If  for  some  reason  this  process  takes  too 
much  time,  then  the  Filter  Data  task  should  continue  the  data  filtering  process  with  the 
current  updated  data.  The  length  of  the  timeout  is  changeable  and  is  dependent  on  the 
speed  of  the  filtering  and  the  acquisition  processes.  The  timeout  value  is  calculated  to  be 
1/60  of  a  second,  but  it  can  be  changed  during  the  system  tests. 
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Figure  26:  System  Deployment  Diagram 

The  QAEF  system  has  two  subsystems  namely  the  Filtering  Subsystem  and  the 


Acquisition  Subsystem.  The  Filtering  Subsystem  is  responsible  for  data  sampling  and  for 
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data  filtering.  This  subsystem  is  composed  of  the  Filter,  Sampler,  Quaternion,  and 
Matrix  classes  in  the  Data  Filtering  package,  Figure  27. 


Figure  27:  Filtering  Subsystem  Structure 

The  active  objects  in  Data  Filtering  package  are  the  Filter  and  Sampler.  The  operations 
of  these  active  objects  in  the  Filtering  Subsystem  are  shown  in  Figure  28.  The  Filter  is 
responsible  for  the  calculations  in  the  filtering  process.  The  Filter  object  requests  data 
from  the  Sampler.  Upon  receiving  the  sampled  data,  it  calculates  the  bias  correction  (the 
Filter  object  must  calculate  the  initial  bias  correction  for  the  first  step  only).  After 
applying  this  bias  error  to  the  sampled  data,  it  starts  the  calculation  process  to  find  an 
estimated  result.  The  result  will  be  printed  on  the  screen  and  will  be  written  into  a  file. 
The  Filter  object  state  model  is  presented  in  Figure  29. 

The  Sampler  is  responsible  for  initializing  the  calibration  values,  getting  data 
from  the  converter,  averaging  this  data,  and  providing  this  sampled  data  to  the  Filter 
along  with  the  time  interval  of  the  samples.  The  calibration  values  are  the  zero  level 
voltages  for  the  sensor  readings.  These  values  can  either  be  found  by  the  Sampler  or  can 
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be  hard  coded.  The  Sampler  object  gets  data  from  the  converter  when  requested  by  the 
Filter.  The  Sampler  samples  the  data  by  averaging,  and  then  sends  the  samples 
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Figure  28:  Filtering  Subsystem  Operations 

and  the  time  interval  to  the  Filter.  The  Sampler  will  wait  for  the  data  transmission  from 

the  double  buffer  into  the  transfer  buffer.  If  this  transmission  exceeds  the  transmission 
time,  then  a  timeout  occurs.  The  state  model  for  the  Sampler  is  presented  in  Figure  30. 
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Figure  29:  Filter  Object  State  Model 


Figure  30:  Sampler  Object  State  Model 
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The  Quaternion  class  supports  the  system  with  quaternion  operations  and  with 
quaternion  arithmetic  as  explained  in  Chapter  IH.  This  class  could  support  any 
simulation  or  tracking  program  using  quaternions. 

The  Matrix  class  supports  the  system  with  necessary  matrix  operations.  These 
operations  are  used  in  the  filtering  process  and  are  special  for  this  design.  It  also  has  the 
ability  to  make  combinations  of  and  conversions  between  quaternions  and  matrices. 

The  Acquisition  Subsystem  is  responsible  for  getting  data  from  the  sensors  and 
for  updating  the  double  buffer  with  this  data.  This  subsystem  is  composed  of  the 
Converter  class  in  the  Data  Acquisition  package,  as  depicted  in  Figure  31. 


package  Data  Acquisition 

Converter 

Figure  31:  Acquisition  Subsystem  Structure 

The  Converter  is  an  active  object.  The  Converter  class  provides  an  interface  between  the 

system  and  the  converter  hardware.  The  Converter  is  responsible  for  initiating  and 
calibrating  the  A/D  converter,  for  monitoring  the  data  collection  process,  for  updating  the 
double  buffer  with  new  data,  and  for  transferring  halves  of  the  double  buffer  into  the 
transfer  buffer.  The  Converter  communicates  with  the  Sampler  during  this  transfer 
process.  It  checks  for  the  timeout  condition  and  monitors  all  events  for  any  possible  error 
in  data  acquisition,  such  as  buffer  overflow  and  buffer  overwrite.  The  Converter 
concurrently  checks  the  channels  for  new  data,  and  checks  for  any  requests  from  the 
Sampler.  The  operations  of  the  Converter  in  the  Data  Acquisition  Subsystem  are  shown 
in  Figure  32. 
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Figure  32:  Acquisition  Subsystem  Operations 

The  Converter  updates  the  double  buffer  when  new  data  is  received  and  it  transfers  the 


data  into  the  transfer  buffer.  The  state  model  for  the  Converter  is  presented  in  Figure  33. 


Figure  33:  Converter  Object  State  Model 
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E.  SYSTEM  IMPLEMENTATION 


The  system  was  implemented  using  the  C++  programming  language  and  the 
Visual  C++  5.0  compiler.  The  attribute  and  the  variable  names  given  are  the  same  as 
those  used  to  define  the  Quaternion  Attitude  Estimation  Filter  in  Chapter  IV.  The 
attributes  and  the  variables  are  commented  and  the  functions  are  explained  along  with 
their  parameters  in  the  function  headers.  Terminology  consistency  between  the 
documentation  and  the  code  were  strived  for  throughout  implementation. 

The  Gauss-Newton  method  and  the  filtering  equations  are  implemented  in  the 
Filter  class  as  they  are  presented  in  Chapter  IV.  Operator  overloading  is  used  in  both  the 
Quaternion  class  and  the  Matrix  class  to  clarify  the  source  code  and  to  provide  for  ease  of 
use.  The  Quaternion  class  has  the  ability  to  perform  every  quaternion  operation 
explained  in  Chapter  HI.  Every  class  has  the  ability  to  print  its  pre-specified  attributes  by 
overloading  the  stream  insertion  operator. 

Utility  functions  to  write  into  and  read  from  a  file  are  provided  for  the  user.  All 
classes  have  default  constructors,  but  the  user  has  the  opportunity  to  initialize  the  objects 
by  reading  the  initial  values  from  a  configuration  file. 

A  function  library,  nidaq32.1ib,  is  used  to  program  the  converter  and  these 
functions  are  explained  with  their  parameters  in  [NATI98A].  The  converter  has  two 
types  of  data  acquisition,  single  buffered  and  double  buffered.  Single  buffered  operations 
are  relatively  simple  to  implement,  and  can  usually  take  advantage  of  the  full  hardware 
speed  of  data  acquisition  device.  However,  more  sophisticated  applications  involving 
larger  amounts  of  data  input  at  higher  rates  require  more  advanced  techniques.  One  such 
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technique  is  double  buffering.  This  allows  continuous,  uninterrupted  input  of  large 
amounts  of  data  [NATI98B].  In  this  implementation,  double  buffering  was  chosen. 

The  converter  is  configured  to  work  in  bipolar  mode,  ±10  V,  with  board  gain  2. 
In  this  configuration,  the  digital  ranges  for  the  actual  voltages  will  be  between  -32768 
and  32767.  These  values  are  used  to  initialize  the  converter  object. 

F.  SUMMARY 

The  QAEF  software  is  designed  to  track  motion  in  all  orientations  in  real  time. 
The  UML  and  its  visual  notations  were  used  to  analyze  and  to  design  it.  The  software  is 
written  in  C++,  with  the  Visual  C++  5.0  compiler,  for  use  on  an  IBM-compatible 
processor.  This  chapter  presented  a  general  overview  of  the  software  development 
process.  The  source  code  is  presented  in  Appendix  C.  The  test  plans  and  the  actual 
testing  of  a  system  are  the  final  steps  in  the  software  development  process.  The  next 
chapter  will  present  the  testing  methodology  and  the  test  results  of  the  QAEF  system. 
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Vn.  SYSTEM  TESTING 


A.  INTRODUCTION 

System  testing  is  the  final  step  in  software  and  hardware  development.  The 
quantitative  tests  focused  on  the  software  and  the  accuracy  of  the  system.  This  chapter 
presents  system  calibration,  testing  methodology  and  the  test  results. 

B.  SYSTEM  CALIBRATION 

To  initialize  the  system  three  values  must  be  provided  to  the  QAEF  object, 
namely  converter  number,  buffer  depth  and  a  constant  gain  value  for  k.  The  converter 
number  specifies  the  converter  to  use  when  there  is  more  than  one  converter.  The  buffer 
depth  indirectly  determines  update  rate.  Up  to  the  point  of  buffer  overwrite  errors,  small 
buffer  depths  will  provide  faster  update  rates.  The  constant  gain  value  k  is  discussed  in 
Chapter  IV.  Throughout  the  quantitative  tests,  k  was  set  to  3  and  a  was  kept  less  than  1. 

Most  of  the  calibration  is  done  within  the  sampler  object.  The  sampler  object  has 
a  self-calibrating  function  namely  findZeroVoltages.  This  function  finds  the  zero  level 
voltages  for  angular  rate  sensors  and  accelerometers.  The  sensor  block  must  be  sitting  on 
a  level  surface  for  several  milliseconds  to  find  these  zero  level  voltages.  Hard  coded 
values  may  also  be  used.  These  hard  coded  values  can  be  determined  by  taking  the 
average  values  of  the  maximum  and  the  minimum  readings  of  each  sensor  in  the  sensor 
block.  The  self-calibration  process  can  be  enabled  or  disabled.  When  the  self-calibration 
is  enabled  the  findZeroVoltages  function  will  initialize  the  sampler,  otherwise  the 
sampler  will  initialize  itself  with  hard  coded  values.  In  actual  tests,  initializing  the  Earth 
magnetic  field  to  the  first  reading  from  magnetometer  obtained  during  calibration 
provided  better  results  and  simplified  the  initialization  process. 
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All  calibration  of  the  A/D  converter  is  done  automatically  by  the  converter  object. 
The  parameters  to  calibrate  the  A/D  converter  are  chosen  to  get  the  best  performance 
from  the  A/D  converter.  These  parameters  can  easily  be  changed  using  the  parameters 
discussed  in  [NATI98A]  and  making  changes  to  the  converter  object. 

C.  TESTING  METHODOLOGY 

The  sensor  block  was  mounted  on  the  tilt  table  using  a  non-ferrous  extension, 
which  has  approximately  the  length  of  a  human  forearm.  The  tilt  table  was  programmed 
to  rotate  45  degrees  at  a  rate  of  10  degrees  per  second  on  every  axis  for  a  series  of  roll, 
pitch  and  yaw  tests.  After  initializing  the  system,  three  rotations  were  completed  by  the 
tilt  table  in  each  test.  During  each  test,  a  15  to  20  second  stabilization  period  followed 
each  movement.  The  scale  factors  and  buffer  depth  values  were  adjusted  to  obtain  the 
desired  update  rate  and  accuracy. 

D.  TEST  RESULTS 

The  following  results  were  obtained  using  the  hardware  and  software  described  in 
Chapters  V  and  VI.  To  achieve  an  update  rate  of  55  Hz,  the  graphical  representation  of 
orientation  was  disabled.  The  update  rate  with  the  graphical  orientation  representation 
and  a  single  processor  was  approximately  15  Hz.  The  roll,  pitch,  and  yaw  test  results  are 
presented  in  Figures  34,  35,  and  36  respectively. 

Estimated  orientation  was  within  1  degree  of  actual  steady  orientation  throughout 
the  tests.  The  smoothness  of  the  graphs  indicates  excellent  dynamic  response.  The  small 
impulses  observed  at  the  starting  points  of  motion  were  hypothesized  to  be  linear 
acceleration  effects  exaggerated  by  a  whipping  motion  of  the  non-ferrous  extension 
block.  It  is  expected  that  adjusting  the  filter  scale  factors  and  gain  values  will  reduce  this 
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Figure  35:  45  Degree  Pitch  Test,  10  deg/sec,  a  =  0.054, 55  Hz. 
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Figure  36:  45  Degree  Yaw  Test,  10  deg/sec,  a  =  0.054, 55  Hz. 
effect  and  improve  the  overall  accuracy  and  dynamic  response.  The  transition  times 

observed  in  the  plots  are  around  4.5-5  seconds  as  expected  for  a  10-degree  per  second 

rotation  rate  to  45  degrees.  In  qualitative  tests,  the  system  was  able  to  track  all 

orientations,  including  those  in  which  pitch  equaled  90  degrees;  the  same  orientations 

normally  cause  singularities  in  Euler  angle  filters.  The  qualitative  tests  also  show  that  the 

system  could  easily  be  combined  with  a  simulation  program  and  track  motion  in  real 

time. 

E.  SUMMARY 

This  chapter  has  described  the  system  calibration,  the  system  testing  methodology 
and  the  qualitative  and  quantitative  dynamic  tilt  table  test  results.  These  tests  show  that 
the  system’s  lag  is  very  low,  and  the  accuracy  is  within  1  degree  when  a  proper 
calibration  is  completed.  The  prototype  system  update  rate  can  be  adjusted  between  15 
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and  55  Hz.  It  appears  to  be  capable  of  tracking  human  motion  in  real  time.  The  final 
chapter  of  this  thesis  will  review  this  research,  reach  some  conclusions  and  make 
recommendations  for  future  work. 
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VIII.  SUMMARY  AND  CONCLUSIONS 


A.  SUMMARY 

Several  techniques  exist  to  track  human  motion  and  many  research  efforts  have 
addressed  body  tracking  systems  and  orientation  filters.  The  most  commonly  used 
method  for  defining  orientation  is  Euler  angles.  Motion  tracking  systems  using  Euler 
angles  are  not  capable  of  tracking  objects  in  all  orientations  due  to  gimbal  lock 
singularities.  An  alternative  method,  the  use  of  unit  quaternions,  has  been  gaining 
popularity  in  the  graphics  community  since  mid  80’ s.  A  quaternion  based  attitude 
estimation  filter  has  been  proposed  to  overcome  these  singularities.  This  thesis  described 
the  development  and  testing  of  a  prototype  inertial  tracking  system  based  on  a  quaternion 
attitude  estimation  filter.  Test  results  indicate  that  human  motions  could  be  efficiently 
tracked  with  this  system. 

B.  CONCLUSIONS 

This  research  continues  that  begun  in  [HENA97].  The  presented  system  is  the 
first  inertial  body  tracking  system  based  on  quaternions  [DURL95,  SKOP96].  Using  unit 
quaternions  prevented  the  singularities  and  increased  computational  efficiency. 

The  system  resolution  is  very  good,  and  accuracy  is  within  1  degree.  Robustness 
and  registration  are  very  good  in  a  homogenous  environment,  but  relatively  strong 
magnetic  fields  will  effect  the  system.  Since  the  system  is  sourceless,  it  can  be  assumed 
that  there  is  no  limit  to  sociability  in  a  very  large  homogenous  environment.  The 
prototype  sensor  is  small  and  it  is  light.  This  makes  the  system  user  friendly  and  easy  to 
use.  Smaller  sensor  blocks,  wristwatch  size,  could  be  produced  with  smaller  sensors  in 
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the  near  future.  The  system  was  constructed  using  low  cost,  off  the  shelf  components. 
System  cost  and  size  would  be  reduced  with  mass  production. 

C.  RECOMMENDATIONS  FOR  FUTURE  WORK 

The  system  will  be  used  as  a  part  of  the  on  going  body  suit  project  [ZYDA97]  and 
could  also  be  used  in  the  NPS  AUV  project  [YUN97].  The  system  and  multiple  sensor 
blocks  can  be  combined  with  the  quaternion  human  model  [USTA99]  for  real  time 
human  motion  tracking  applications.  Using  multiple  sensor  blocks  will  decrease  the 
system  performance  on  a  single  processor  system  To  prevent  this,  a  multiprocessor 
architecture  should  be  developed  for  full  human  body  motion  tracking  applications. 
Tracking  humans  in  large  environments  will  require  a  wireless  tracking  system. 
Developing  a  wireless  tracking  system  architecture  should  be  considered. 

The  system  has  a  self-calibrating  module  or  the  scalar  numbers  can  be  hard  coded 
for  system  calibration.  An  improved  calibration  procedure  should  be  developed  to 
achieve  better  results.  The  filter  can  be  used  with  other  types  of  sensors.  Integrating 
better  sensors  into  the  system  should  be  considered. 
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APPENDIX  A.  DERIVATION  OF  X  MATRIX 


The  X  matrix  is  defined  by  (Eq.  4. 13)  as 


VT  _  HR 

j  "L^j 


(Eq.  A.1) 


The  elements  of  the  4x6  matrix  come  from  the  partial  derivatives  of  the 
components  of  the  computed  measurement  vector,  y(q) ,  given  by  (Eq.  4.8) 


y(q)  =  (q  ^q.q  *nq) 


(Eq.  A.2) 


and  as  given  in  (Eq.  4.6)  and  (Eq.  4.7) 


r  *  l  a 
h-q  mq 


(Eq.  A.3) 


L  A  1  A 

b-q  nq 


(Eq.  A.4) 


Therefore,  taking  the  partial  derivative  of  this  equation  with  respect  to  q0 ,  the 


result  is 


dy  d  -i  „  .-i 
3T-  =  3^(?  mq,q  nq) 
dq0  oqo 


(Eq.  A.5) 


Applying  the  product  rule  to  (Eq.  A.5),  it  follows  that 


dy  dq  ~  dq  dq  „  „-l  dq 

-— =  ——mq  +  q  m-—,——nq  +  q  n— 
oqo  l  dq0  dq0  dq0  dq0 


(Eq.  A.6) 


where 


=  (1000) 


(Eq.A.7) 
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and 
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Likewise,  for  qx,q2,  and  q3\ 
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where  the  corresponding  quaternion  partial  derivatives  are 

dq 
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dq 
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and,  the  partial  derivatives  of  the  inverse  are  given  by 
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(Eq.  A.8) 

(Eq.  A.9) 

(Eq.  A.  10) 

(Eq.  A.  11) 

(Eq.  A.  12) 

(Eq.  A.  13) 

(Eq.  A.  14) 

(Eq.  A.  15) 

(Eq.  A.  16) 

(Eq.  A.  17) 
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The  partial  derivatives  as  defined  in  (Eq.  A.6),  (Eq.  A.9),  (Eq.  A.  10),  and  (Eq. 
A.  11)  result  in  the  partial  derivatives  of  the  m  and  n  vectors  with  respect  to  qo,qx>h’ 
and  <?3,  respectively. 

Taking  the  results  computed  above,  the  X  matrix  can  be  constructed  as  follows 


X  = 


dy  dy  dy  dy 


(Eq.  A.  18) 


J6x4 


L<2?0  dqx  dq2  dq: 3 

Note  that  the  partial  derivatives  are  column  vectors  in  the  X  matrix,  and  that  the 
transpose  ofXis  required  when  used  in  the  filter.  Thus,  (Eq.  A.  18)  becomes  [HENA97] 
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Then  the  transpose  of  the  X  matrix  is 
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(Eq.  A.  19) 
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APPENDIX  B.  DERIVATION  OF  GRADIENT  OF  THE  ERROR 


CRITERION  FUNCTION 

In  the  filter,  the  gradient  of  the  error  criterion  function  </>(q),is  given  by  (Eq.4.17) 

where  <p(q ),  is  given  by  (Eq.4. 14) 

<M)u\  =  tfwslilfci  (Eq-  b.2) 

In  order  to  simplify  the  derivation  of  the  gradient,  consider  the  the  case  of  one 
dimensional  orientation  vectors,  where  the  measured  vector  is  y0  =  (fy )  the 

A 

calculated  vector  is  y(q)  =  {h{) .  The  error  then  becomes 

€(q)  =  hl-h1  (Eq.  B.3) 

and 


(j>{q)  =  e2  =  (hl-h1)2  =  hi  -  2\hx  +  hi 


(Eq.  B.4) 


Taking  the  partial  derivative  of  (Eq.B.4)  with  respect  to  q$  yields 


A  A  A  A 

dd  -  dh-\  dhi  dh  -  _  dh\  ,  . 

-r?-  =  2h1-3±-2hl^-  =  2-^L(hl-hl)  =  2-^L£(q) 
dqQ  dq0  dq0  dq0  dqQ 


(Eq.  B.5) 


Likewise,  for  the  partial  derivatives  with  respect  to  q\,q[2>  and q^  [HENA97]. 


Thus,  extending  this  result  to  6  dimensional  case  and  arranging  the  partial  derivatives  in 
matrix  form  yields  the  derivation  of  the  gradient  of  the  error  criterion  function. 
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The  modeling  error  can  be  written 
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(Eq.  B.6) 


Based  on  this  error,  <p(q)  becomes 

<t>{q)  =  {hi -hi)2  +  (4 -h2)2  +  (h3-h3)2+(bi - bi )2  +  (4  -b2)2  +(4 -b3)2  (Eq.  B.7) 
Taking  the  partial  derivative  of  (Eq.  B.7)  with  respect  to 
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we  obtain 
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(Eq.  B.9) 


Rows  two  through  four  are  produced  by  taking  the  partial  derivative  of  (j){q)  with  respect 
to  qi,q2,  and  q3. 
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Thus, 
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which  can  be  rewritten  [BACH97A] 

V<Kq)  =  2XT£(q) 

where  the  transpose  of  the  X  matrix  is  as  defined  in  Appendix  A. 


(Eq.  B.10) 


(Eq.  B.ll) 
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APPENDIX  C.  SOURCE  CODE 


A.  QAEF.H 


//******************************************************************* 
//**************★***************************************** *********** 
// 

//  File  :  Qaef.h 
//  Author  :  ildeniz  DUMAN 

//  Project  Name  :  Quaternion  Attitude  Estimation  Filter 
//  Operating  Environment  :  MS-Windows  95 
//  Compiler  :  Visual  C++  5.0 
//  Date  :  01/09/99 
// 

//  Description  : 

// 

//  -  Header  file  for  Qaef 

// 

//  Inputs  :  None 

// 

//  Outputs  :  None 

// 

//  Process  :  None 

// 

//  Assumption  :  None 
// 

//  Warnings  during  compilation  :  None 
//*************************★***************************************** 
//*********************************************************  *.*  ******** 

#ifndef  _ QAEF_H _ 

#define  _ QAEF_H _ 


#include 
# include 
# include 
#include 
# include 


<iostream.h> 
<iomanip.h> 
"converter .h" 

" filter. h" 

" quaternion . h " 


class  Qaef{ 
public : 

//  default  constructor 

Qaef  (int  converterNo  , unsigned  int  transferBuf Size  =  400, 
unsigned  int  kValue  =  150) ; 

//  destructor 
-Qaef ( ) ; 

//  starts  the  filtering  process 
double  start(); 
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//  stops  the  converter 
void  stop(); 

//  returns  the  estimated  quaternion 
Quaternion  getResultO; 

private : 

//AD  converter 
Converter  *  qConverter; 

//  Filter 
Filter  *  qFilter ; 


} ; 


#endif 

//  end  of  file  Qaef.h 
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B.  QAEF.CPP 


//*************************************************★***************** 
//******************************************************* ************ 
// 

//  File  :  Qaef.cpp 
//  Author  :  ildeniz  DUMAN 

//  Project  Name  :  Quaternion  Attitude  Estimation  Filter 
//  Operating  Environment  :  MS-Windows  95 
//  Compiler  :  Visual  C++  5.0 
//  Date  :  01/09/99 
// 

//  Description  : 

// 

//  -  Source  file  for  Qaef 
II-  Initializes  A/D  converter  and  filter 
//  -  Returns  estimated  result 
// 

//  Inputs  :  None 
// 

//  Outputs  :  None 
// 

//  Process  :  None 
// 

//  Assumption  :  None 
// 

//  Warnings  during  compliation  :  None 

//******************************************************************* 

//*************************************★***************************** 

#include  <stdio.h> 

#include  <stdlib.h> 
tinclude  "qaef.h" 

# include  "converter .h" 

#include  " filter. h" 

# include  " quaternion . h" 


// 


// 


// 


Function: 

Qaef ( ) 

Return  Value: 

None 

Parameters : 

None 

Description: 

Default  constructor 

Qaef :: Qaef (int  deviceNo,  unsigned  int  bufSize,  unsigned  int  k  ) 
iqConverter  (new  Converter (deviceNo, bufSize) ) , 
qFilter  (new  Filter (qConverter,  k) ) 

{ 

}//end  Constructor 


97 


// - 

//  Function:  -Qaef ( ) 

//  Return  Value:  None 

//  Parameters:  None 

//  Description:  Destructor 

// - 

Qaef : : -Qaef ( ) 

{ 

qConverter->~Converter { ) ; 
qFilter->~Filter 0 ; 
cerr<< "  QAEF  dele  ted "  «endl  ; 
}//  end  destructor 


// - 

//  Function:  start () 

//  Return  Value:  None 
//  Parameters:  None 

//  Description:  Starts  filtering  process 

// - - - 

double  Qaef :: start ( ) 

{ 

qConverter->start ( ) ; 

return  qFilter->start ( )  ; 

}//end  start () 


// - 

//  Function:  getResultO 

//  Return  Value:  Quaternion  -  estimated  result 

/ /  Parameters :  None 

//  Description:  Returns  the  estimated  result 

// - 

Quaternion  Qaef : :getResult ( ) 

{ 

return  qFilter->estimateRotation ( ) ; 

}//  end  getResultO 


// - 

//  Function:  stop  0 

/ /  Return  Value :  None 

//  Parameters:  None 

//  Description:  Stops  the  converter 

// - 

void  Qaef :: stop () 

{ 

qConverter->stop ( ) ; 

return; 

}//  end  stop 

//end  of  file  Qaef.cpp 
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c 


FILTER.H 


//*****************  ****************************  ********************** 
//******************************************************************* 
// 

//  File  :  Filter. h 
//  Author  :  ildeniz  DUMAN 

//  Project  Name  :  Quaternion  Attitude  Estimation  Filter 
//  Operating  Environment  :  MS-Windows  95 
//  Compiler  :  Visual  C++  5.0 
//  Date  :  01/09/99 
// 

//  Description  : 

// 

//  -  Header  file  for  Filter. cpp 

//  -  Filters  data 

//  -  Estimates  an  result 

// 

//  Inputs  :  None 
// 

//  Outputs  :  None 

// 

//  Process  :  None 

// 

//  Assumption  :  None 

// 

//  Warnings  during  compilation  :  None 

//******************************************************************* 

//******************************************************************* 

#ifndef  _ FILTER_H _ 

#def ine  _ FILTER_H _ 


# include 
#include 
#include 
#include 
#include 
# include 


<iostream.h> 
<iomanip .h> 
"matrix.h" 
"quaternion. h” 
M sampler .hH 
"converter .h" 


class  Filter{ 

//  overloaded  operator« 

friend  ostream  &operator« (ostream  &# const  Filter  &) ; 
public: 

//  default  constructor 

Filter  (Converter  *,  unsigned  int  k  =  5); 

//  destructor 
-Filter ( ) ; 


//  copy  constructor 
Filter  (const  Filter  &)  ; 
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//  starts  filtering  process 
double  start ( ) ; 

//  estimates  a  new  rotation 
Quaternion  estimateRotation ( ) ; 

private : 

Sampler  *mySampler;  //my  server  sampler 

Quaternion  n,  //  Earth’s  unit  magnetic  field 

m,  //  Earth’s  unit  gravity 

h,  //  local  vertical  from  accelerometer 

b,  //  local  magnetic  field  from  magnetometer 

bw,  //  rate  ,  from  angular  rate  sensor 

deltaq,  //  Gauss  Newton  iteration 

qDot,  //  derivation  of  angular  rate 

hHat,  bHat,  //in  computed  measurement  vector 
qHat;  //  estimated  quaternion 

//  partial  derivatives 
const  Quaternion  QHATZERO, 

QHATONE ,  ONEINV, 

QHATTWO ,  TWOINV, 

QHATTHREE ,  THREEINV; 

Matrix  x,  //X  matrix 

xTranspose,  //X  transpose  matrix 

errorqHat,  //  error  q  hat 

errorTranspose,  //  error  transpose 

temp, tempi;  //  temp  matrices  for  calculations 

int  tau;  //for  bias  error 

bool  selfcalibration;  //  for  Sampler  to  find  zero  level  voltages 

double  sampleWeight ,  //  for  bias  error 

biasWeight , 

deltaT,  //  time  between  two  sampling  in  seconds 

*  biasError,  //  bias  errors  for  angular  rates 

*  samples,  //  received  readings  from  sampler 

kValue,  //  k  value,  will  be  a  constant 

errorMagnetitude;  //  error's  magnetitude 

//  gets  new  sampled  data  from  sampler 
void  getNewSample  ( ) ; 

//  calculates  initial  bias  error 
void  initialBiasError ( ) ; 

//  corrects  bias  error  by  low  pass  filtering 
void  correctBiasError () ; 

//  sets  the  quaternions  with  new  data 
void  setElements ( ) ; 
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//  calculates  error 
void  calculateError ( ) ; 

//  creates  X  Transpose  matrix 
void  createXTranspose ( ) ; 

//  calculates  delta  q  quaternion 
void  calculateDeltaQ ( ) ; 

//  calculates  qDot  quaternion 
void  calculateqDot ( ) ; 

//  calculates  final  result 
void  calculateResult ( ) ; 

//  converts  the  final  matrix  solution  in  to  quaternion 
void  convertToDeltaq (Matrix  Sc)  ; 

//  writes  results  into  a  file 
void  logResults ( ) ; 


#endif 

//  end  of  file  Filter. h 
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D 


FELTER.CPP 


//******************************************************************* 

// 

//  File  :  Filter. cpp 
//  Author  :  ildeniz  DUMAN 

//  Project  Name  :  Quaternion  Attitude  Estimation  Filter 
//  Operating  Environment  :  MS-Windows  95 
//  Compiler  :  Visual  C++  5.0 
//  Date  :  01/09/99 
// 

//  Description  : 

// 

//  -  Source  file  for  Filter 
//  -  Filters  data 
//  -  Estimates  an  result 
// 

//  Inputs  :  None 

// 

//  Outputs  :  None 
// 

//  Process  :  None 
// 

//  Assumption  :  None 
// 

//  Warnings  during  compilation  :  None 
//***********************************★*******************★******★★*** 
//***★*★**★★****★*****************************★★*★******************* 

#include  <math.h> 
tinclude  <string .h> 

#include  <assert.h> 

# include  <sys/timeb.h> 

#include  <time.h> 

#include  <fstream.h> 

#include  <stdlib.h> 

#include  " f ilter .h" 

#include  "matrix . h" 

#include  "quaternion .h" 

#include  " sampler .h" 

#include  "util .h" 

of stream  f results; 

// - 

//  Function:  Filter () 

//  Return  Value:  None 
/ /  Parameters :  None 

//  Description:  Default  constructor 

// - 

Filter : :Filter  (Converter  *  con,  unsigned  int  kVal) 

:n (Quaternion  ("n",  0  ,  0  ,  60  ,  -15)), //for  Monterey, CA, in  Euler  form 
m (Quaternion  ( "m" ,0.0, 0.0, 0.0, -1.0)), 
h( Quaternion  ( "h" ) ) , 
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b (Quaternion  ( "b" ) ) , 

bw (Quaternion ( "bw" ) ) , 

del tag (Quaternion  ("delta  q" ) ) , 

qDot (Quaternion  ( "q  dot" ) ) , 

hHat (Quaternion  ( "h  hat" ) ) , 

bHat (Quaternion  ( "b  hat" ) ) , 

qHat (Quaternion  ( "Result" ,1.0,0. 0,0. 0,0.0)), 

QHATZERO (Quaternion ( "qhat  zero" ,1.0,0. 0,0. 0,0.0)), 

QHATONE (Quaternion  ( "qhat  one" ,0.0, 1.0, 0.0, 0.0)), 

ONEINV (Quaternion  ("qhat  one  inv" , 0 . 0 , -1 . 0 , 0 . 0 , 0 . 0) ) , 

QHATTWO (Quaternion  ( "qhat  two" ,0.0, 0.0, 1.0, 0.0)), 

TWOINV (Quaternion  ("qhat  two  inv" , 0 . 0 , 0 . 0 , -1 . 0 , 0 . 0) ) , 

QHATTHREE (Quaternion  ("qhat  three" , 0 . 0 , 0 . 0 , 0 . 0 , 1 . 0) ) , 

THREEINV( Quaternion  ("qhat  three  inv" , 0 . 0 , 0 . 0 , 0 . 0 , -1 . 0 ) ) , 
x (Matrix  ( "X" ,6,4)), 

xTranspose (Matrix  ( "X  Transpose" ,4,6)), 
errorqHat  (Matrix  ( " error  q  hat ”,6,1)), 
errorTranspose (Matrix  ( "Error  Transpose" ,1,6)), 
errorMagnetitude (0.0) , 
temp  (Matrix  ( "temp" , 4, 4) ) , 
tempi  (Matrix  ( " tempi" , 4, 1) ) , 
tau (100) ,  deltaT (0 . 1) ,kValue (kVal) , 
sampleWeight (0.0) ,  biasWeight (0.0) , 

selfcalibration (true)  //  sampler  will  find  zero  level  voltages 

{ 


writeFile (f results, "FilterResults.dat") ; 

mySampler  =  new  Sampler (con) ; 

biasError  =  new  double  [3]; 
assert  (biasError  !=0); 

samples  =  new  double  [9] ; 
assert  (samples  !=0); 

for  (int  i=0 ; i<3 ; i++) { 

biasError  [i]  =  0.0; 

} 

for  (int  k=0;k<9;k++) { 

samples  [k]  =  0.0; 

} 

qHat.normalizef)  ; 

//  find  the  Earth's  unit  magnetic  field  ,  n 
Quaternion  tt ( " tt " , 0 , 1 , 0 , 0 ) ;  //  just  a  temp  quaternion 
n=n. toQuaternion ( ) ;  //  convert  n  to  Quaternion 

n=n. rotation (tt) ;  //  rotate  n  with  (0  1  0  0) 

/  /cerr«n«endl ; 
qHat .normalize ( )  ; 

//  will  be  set  to  b  ‘ 

n. setQuaternion (0 .0,0.79  ,0.0055  ,0.61); 

/ /n. normalize ( ) ; 
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cout  «m«endl; 

cout  «"K  =  "«kValue«endl; 

cout« "  initial  "  «qHat«endl  ; 

cout  «MFilter  :  Initialized  "«endl; 

}//end  Constructor 


// - 

//  Function:  -Filter () 

//  Return  Value:  None 
/ /  Parameters :  None 

//  Description:  Destructor 

// - 

Filter: : -Filter () 

{ 

cout  «" deleting  Filter  "«endl ; 
delete  []  samples; 
delete  []  biasError; 
f results . close ( )  ; 
mySampler->~Sampler ( ) ; 

}//end  destructor 


// - 

//  Function:  Filter (Filter  &) 

//  Return  Value:  None 
//  Parameters:  Filter 

//  Description:  Copy  constructor 

// - 

Filter :: Filter (const  Filter  &F) 

{ 

cerr  «"copy  constructor  NOT  implemented"; 
}//  end  copy  constructor 


// - 

//  Function:  start () 

//  Return  Value:  delta  t  -  double 

//  Parameters:  None 

//  Description:  Starts  the  filtering  process /calculates  the  initial 

//  bias  error 

// - 

double  Filter :: start ( ) 

{ 

cout  «  " \nFilter  :  Filtering  started" «endl ; 
mySampler->findZeroVoltages (selfCalibration) ; 

.  getNewSample  ( )  ; 
initialBiasError ( )  ; 
correctBiasError ( )  ; 
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setElements  ( )  ; 

//  sets  Earth's  magnetic  field  to  the  first  reading 
n=b; 

calculateError ( ) ; 

createXTranspose ( ) ; 

calculateDeltaQ ( ) ; 

calculateqDot ( )  ; 

calculateResult ( ) ; 

//  logResults () ; 

return  deltaT; 

}//end  start () 


// - 

//  Function: 

//  Return  Value: 
//  Parameters: 

//  Description: 
// 

// - 


estimateRotation ( ) 

qHat  -  quaternion  -  estimated  result 
None 

Estimate  new  rotation,  must  call  itself  for 
continuous  estimation 


Quaternion  Filter: : estimateRotation ( ) 

{ 

getNewSample ( ) ; 


correctBiasError ( ) ; 


setElements ( ) ; 
calculateError ( ) ; 


createXTranspose ( ) ; 
calculateDeltaQ ( )  ; 
calculateqDot ( ) ; 
calculateResult ( ) ; 
//  logResults (); 


return  qHat; 

}//  end  estimateRotation ( ) 
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// - 

//  Function:  initialBiasError ( ) 

//  Return  Value:  None 

/ /  Parameters :  None 

//  Description:  Calculates  initial  bias  errors  and  sets  them 

// - 

void  Filter: : initialBiasError () 

{ 

int  biasNumber  =  tau/10; 

//  check  for  alpha 

if  ( ( (deltaT  *  kValue)  <  1.0)  &&  ( (deltaT  *  kValue)  >  0.0) ){ 
cerr  «  "Alpha  is  "<< (deltaT*kValue) «endl ; 

}else  { 

cerr  <<" WARNING  Filter  :  Alpha  is  "«  (deltaT*kValue) 

«  "deltaT  "«deltaT«"  K  "  «kValue«endl ; 

} 

for  (int  i=0 ; i<biasNumber ; i++) { 

biasError [0]  +=  samples [0] /biasNumber ;  //angular  rate  p 

biasError[l]  +=  samples [1 ] /biasNumber ;  //angular  rate  q 

biasError[2]  +=  samples  [2 ]  /biasNumber ,*  //angular  rate  r 

} 

biasError [ 0 ]  =  -biasError [  0  ]  ; 
biasError [ 1 ]  =  -biasError [ 1 ] ; 
biasError [2]  =  -biasError [ 2 ] ; 

return; 

}//  end  initialBiasError ( ) 


// - 

//  Function:  correctBiasError ( ) 

//  Return  Value:  None 
//  Parameters:  None 

//  Description:  For  every  reading  corrects  angular  rates  with  the 

//  initial  bias  error 

// - 

void  Filter :: correctBiasError ( ) 

{ 

sampleWeight  =  deltaT/tau; 

biasWeight  =  1 -sampleWeight; 


for  (int  i=0 ; i<3 ; i++) { 

samples [i]  +=  (biasWeight  * 

} 


biasError [ i ] ) - ( sampleWeight 
*  samples  [i] ) ; 


return; 

}//  end  correctBiasError ( ) 
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// - 

/ /  Function :  getNewSample ( ) 

//  Return  Value:  None 
/ /  Parameters :  None 

//  Description:  Calls  Sampler  to  get  new  averaged  reading 

// - - - 

void  Filter :: getNewSample ( ) 

{ 

mySampler->getData (samples,  delta!*)  ; 

/* 

for  (int  k=0;k<9;k++) { 

if  (k==0)  cerr«*'\nBody  Rates  [ 

if  (k==3)  cerr«" Accelerometer  [ 

if  (k==6)  cerr«  "Magnetometer  [ 

cerr«samples  [k]  «"  "  ; 
if  ( (k==2 )  ||  (k==5) ) { 

cerr« "  ]  "  «endl  ; 

} 

} 

cerr«"  ]  "«endl; 

*/ 

return ; 

} / /  end  getNewSample ( ) 


// - 

/ /  Function :  setElements ( ) 

//  Return  Value:  None 
/ /  Parameters :  None 

//  Description:  set  b,h,bw  with  new  readings 

// - 

void  Filter : : setElements ( ) 

{ 

//reset  angular  rates 

bw.setQuaternion(0.0,  samples [0] ,  samples [1] ,  samples [2] ) ; 
//reset  accelerometer  measurements 

h. setQuaternion (0 . 0, samples [3 ] , samples [4] , samples [5] ) ; 
h. normalize ( ) ; 

//reset  magnetometer  measurements 

b. setQuaternion (0 . 0, samples [6] , samples [7] , samples [8] ) ; 
b. normalize ( ) ; 

return ; 

} / /  end  setElements ( ) 
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// - 

//  Function:  calculateError { ) 

//  Return  Value:  None 
/ /  Parameters :  None 

//  Description:  Calculates  error  vector 

// - 

void  Filter :: calculateError ( ) 

{ 

static  Quaternion  errH( "error  h" ); 

static  Quaternion  errB ("error  b"); 

hHat  =  qHat .toBody (m) ; 

bHat  =  qHat . toBody (n) ; 

errH  =  hHat  -  h; 

errB  =  bHat  -  b; 

errorMagnetitude  =  errH. dot Product (errH)  +  errB . dotProduct (errB) ; 
errorqHat . insertColQuaternion  (  (hHat-h)  ,  (bHat-b)  ,  0) ; 

return; 

}//  end  calculateError ( ) 


// - 

//  Function:  createXTranspose ( ) 

//  Return  Value:  None 

//  Parameters:  None 

//  Description:  Creates  X  Transpose  matrix 

If - 

void  Filter :: createXTranspose ( ) 

{ 

xTranspose. insertRowQuaternion (m.partialDerivative (qHat , QHAT ZERO, 
QHATZERO) , n .partialDerivative (qHat , QHATZERO, QHATZERO)  ,  0) ; 
xTranspose. insertRowQuaternion (m.partialDerivative (qHat , QHATONE, 
ONEINV)  ,n. partialDerivative (qHat, QHATONE, ONEINV)  ,  1) ; 
xTranspose . insertRowQuaternion (m.partialDerivative (qHat , QHATTWO, 
TWOINV)  ,  n. partialDerivative (qHat, QHATTWO, TWOINV)  ,  2) ; 
xTranspose . insertRowQuaternion (m.partialDerivative (qHat , QHATTHREE 
, THREEINV)  ,n. partialDerivative (qHat, QHATTHREE, THREEINV)  ,  3) ; 
return; 

} / /  end  createXTranspose ( ) 

// - 

//  Function:  calculateDeltaQ ( ) 

//  Return  Value:  None 
//  Parameters:  None 

//  Description:  Calculates  delta  q  in  matrix  form 

// - 

void  Filter: : calculateDeltaQ ( ) 

{ 

xTranspose . transpose (x) ; 
temp  =  (xTranspose  *  x) ; 
temp  =  temp . invert ( ) ; 

tempi  =  temp  *  (xTranspose*errorqHat ) ; 

convertToDeltaq (tempi) ; 

return; 

}//  end  calculateDeltaQ ( ) 
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// - - 

//  Function:  convertToDeltaQ ( ) 

//  Return  Value:  None 

//  Parameters:  MAT  -  calculated  delta  q  matrix 

//  Description:  Converts  deltaQ  4x1  in  to  quaternion 

// - - 

void  Filter : : convertToDeltaq (Matrix  &  MAT) 

{ 

if  ( (MAT . getRow ( ) ==4 )  &&  (MAT.getCol ( ) ==1) ) { 

deltaq . setQuaternion (MAT . getElement (0,0), MAT . getElement (1,0), 


MAT . getElement (2,0), MAT . getElement (3,0)); 

deltaq  =  (~1  *  kValue) *deltaq; 

}  else  { 

cerr  «" Error  in  Filter:  matrix  can  not" 

«"  be  converted  into  quaternion "«endl; 


} 


return; 

} / /  end  convertToDeltaQ ( ) 


// - 

//  Function:  calculateqDot ( ) 

//  Return  Value:  None 

/ /  Parameters :  None 

//  Description:  Calculates  qDot  quaternion 

// - 

void  Filter :: calculateqDot ( ) 

{ 

qDot  =  0.5  *(qHat  *  bw) ; 
return; 

} / /  end  calculateqDot ( ) 


// - 

//  Function:  calculateResult ( ) 

//  Return  Value:  None 
/ /  Parameters :  None 

//  Description:  Finds  the  estimated  result 

// - 

void  Filter :: calculateResult () 

{ 

qHat  =  qHat  +  ( (qDot  *  deltaT)  +  (deltaT  *  deltaq) ) ; 
qHat .normalize ( ) ; 

return; 

}//  end  calculateResult ( ) 
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// - 

//  Function:  calculateResult ( ) 

//  Return  Value:  None 
/ /  Parameters :  None 

//  Description:  Finds  the  estimated  result 

// - 

void  Filter :: logResults ( ) 

{ 

static  int  steps  =  1; 

f resul ts«steps« "  " «qHat« "  Error  =  " «errorMagneti tude«endl  ? 
steps++; 

return; 

} 

// - 

//  Function:  (friend)  operator«() 

//  Return  Value:  ostream  object 
//  Parameters:  output  -  ostream  object 

//  q  -  Filter  to  print 

//  Description:  Prints  the  Filter  in  a  form,  should  be  written  out 

//  of  class 

If - 

ostream  &operator« (ostream  ^output,  const  Filter  &q) 

{ 

output  «  ■  [  '  <<"Filter '  s  members «q. n<<q.m«q.h<<q.b<< 
q .  bw«q .  del  taq«q .  qDot«q .  hHat« 
q .  bHat«q .  qHat<<q .  x«q .  xTranspose« 
q .  errorqHat«q.  errorTranspose«endl ;  ; 

return  output; 

}//  end  operator<< 

//end  of  file  Filter. cpp 
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E, 


CONVERTERS 


//******************************************************************* 

//******************************************************************* 

// 

//  File  :  Converter. h 
//  Author  :  ildeniz  DUMAN 

//  Project  Name  :  Quaternion  Attitude  Estimation  Filter 
//  Operating  Environment  :  MS-Windows  95 
//  Compiler  :  Visual  C++  5.0 
//  Date  :  01/09/99 
// 

//  Description  : 

// 

//  -  Source  file  for  Converter 
//  -  Receives  data  from  sensors 
//  -  Updates  the  double  buffer 
// 

//  Inputs  :  None 

// 

//  Outputs  :  None 

// 

//  Process  :  None 

// 

//  Assumption  :  None 
// 

//  Warnings  during  compliation  :  None 
//***************** ************************************************** 
//******************************************************************* 

#ifndef  _ CONVERTERS _ 

tdefine  _ CONVERTERS _ 

#include  <iostream.h> 

#include  <iomanip.h> 

#def ine  M_PI  3.14159265358979323846 

#def ine  GRAVITY  32.2185  //feet  per  second 


class  Converter { 

//  overloaded  operator« 

friend  ostream  &operator«  (ostream  const  Converter  Sc); 
public : 

//  default  constructor 

Converter  (int  deviceNum,  unsigned  int  rows=300); 

//  destructor 
-Converter ( ) ; 

/ /  starts  the  converter 
void  start (); 


in 


//  checks  if  half  buffer  is  ready 
bool  isHa If Ready ( ) ; 

//  transfers  half  of  the  double  buffer 
void  transferData (short  *); 

//  stops  the  converter 
void  stop ( ) ; 

//  passes  the  configuration  data 

void  getConf  igData  (unsigned  short  &, unsigned  short  &,int  &,int  &, 
double  Sc)  ; 


private : 


long  bufferSize, 

// 

buffer  size 

timeOut; 

// 

time  out 

int  deviceNumber ; 

// 

device  number 

short  *  channelVector , 

// 

channels 

*  gainVector, 

// 

gains  for  channels 

*  doubleBuf fer ; 

// 

double  buffer 

double  rate; 

// 

scan  rate 

short  status, 

// 

returned  status  of  converter  functions 

numberOf Channels , 

// 

number  of  channels 

half Ready, 

// 

half  ready 

daqStop, 

// 

DAQ  stop 

sampleT_JBase , 

// 

sample  time  base 

scanT_Base, 

// 

scan  time  base 

units. 

// 

used  units  during  scanning 

dBEnable, 

// 

flag,  double  buffer  enable 

inputMode , 

// 

converter  input  mode 

inputRange , 

// 

input  range 

polarity. 

// 

input  polarity 

drive, 

// 

specifies  drive 

gain; 

// 

gain  for  scanning 

//  data  points  to  tranfer  to  transfer  buffer 
unsigned  long  pointsTransfer ; 


unsigned  short  samplelnterval , 
scanlnterval , 
maxRange ; 

bool  ready; 

//  initialize  converter 
void  initConverter ( ) ; 

//  calibrates  converter 
void  calibrate(); 


//  sample  interval 
//  scan  interval 
//  max  reading  range 

//  flag  to  indicate  half  ready 
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//  sets  events 
void  setEvents ( ) ; 

//  restarts  the  converter 
void  restart { ) ; 

//  prints  the  initial  values 
void  printlnitialValues ( ) ; 


#endif 

//  end  of  file  Converter. h 
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F, 


CONVERTER.CPP 


//********************************************************************* 

//********************************************************************* 

// 

//  File  :  Converter . cpp 
//  Author  :  ildeniz  DUMAN 

//  Project  Name  :  Quaternion  Attitude  Estimation  Filter 
//  Operating  Environment  :  MS-Windows  95 
//  Compiler  :  Visual  C++  5.0 
//  Date  :  01/09/99 
// 

//  Description  : 

// 

//  -  Source  file  for  Converter  class 
//  -  Receives  data  from  sensors 
//  -  Updates  the  double  buffer  with  this  data 
//  -  Transmits  the  half  buffer 
// 

//  Inputs  :  None 
// 

//  Outputs  :  None 
// 

//  Process  :  None 
// 

//  Assumption  :  None 
// 

//  Warnings  during  compliation  :  None 
//**************************★*★***********★************************** 
//*****************★**************★*★*****★*****★******************** 

#include  <assert.h> 

# include  <iomanip.h> 

#include  " converter . h" 

#include  "nidaq.h" 

# include  "nidaqcns .hM 
# include  "util .h" 


// - 

//  Function:  Converter ( ) 

//  Return  Value:  None 
//  Parameters:  None 

//  Description:  Default  constructor 

// - 

Converter : Converter (int  deviceNo,  unsigned  int  rowNo) 

:  numberOf Channels (9 ) ,  //  to  scan 

timeOut  (180) ,  //  time  out  value 

status (0),  //  initial  status 

sampleT_Base (-3) ,  //  board  resolution, will  be  overwritten  by  DAQ_Rate 
scanT_Base (-3 ) ,  //  20  KHz 

samplelnterval (100) ,  //  will  be  overwritten  by  DAQ_Rate 

scanlnterval (0) ,  //  no  time  interval  between  conversions 

dBEnable(l),  //  enable  double  bufferring 

half Ready (0) ,  //  check  for  half  ready 
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daqStop(O) , 
rate  (20000), 
inputMode ( 1 )  , 
inputRange ( 0 ) 
polarity (0 )  , 
drive (1)  , 
gain (2) , 
ready  (false) 
{ 


//  check  if  DAQ  stopped 
//  DAQ  rate 

//  0  -  DIFF,  1  -  RSE,  2  -  NRSE 
//  (ignored) 

//  0  -  -10/+10  1-  0/+10 
//  drive  AI  sense  to  onboard  ground 
//  channels  have  the  same  gain 
//  flag  for  half  ready 


if  (  (rowNo  >  0)  ScSc  (deviceNo  >  0)){ 

cerr«"  Please  wait  while  calibrating  the  converter 
# "  <<deviceNo«endl  ; 

deviceNumber  =  deviceNo;  //  device  number 

//  units  used  to  express  rate  0  -  pts/sec  ;  1  -  sec/pts 

units=0 ; 

//  the  size  of  the  double  buffer 
bufferSize  =  rowNo  *  numberOf Channels  *  2; 


//  points  to  transfer  to  transfer  buffer 
point sTransfer  =  bufferSize  /  2; 

//  channels  to  scan 

channelVector  =  new  short  [numberOf Channels] ; 
assert  (channelVector  !=  0) ; 

//  gains  per  channel 

gainVector  =  new  short  [numberOf Channels] ; 
assert  (gainVector  !=  0) ; 

//  double  buffer,  cirricular  buffer 
doubleBuf fer  =  new  short  [bufferSize] ; 
assert  (doubleBuf fer  !=  0)  ; 

for  (int  i=0 ; i<numberOf Channels ;i++) { 
gainVector [ i ] =1 ; 

} 

switch  (polarity)  { 
case  0 : 

maxRange  =  32767; 

break; 
case  1: 

maxRange  =  32767  *2+1; 

break; 

default: 

cerr«"  ERROR  :  Unknown  polarity\nM  ; 

break; 

} 

calibrate ( ) ; 
initConverter ( )  ; 
printlnitialValues  ( ) ; 

}else{ 

cerr  «  "Converter  can  not  initialized,  " 

«  "buffer  row  no  must  be  greater  than  0"«endl; 

} 

}//end  Constructor 
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// - 

/ /  Function :  -Converter  { ) 

//  Return  Value:  None 

/ /  Parameters :  None 

//  Description:  Destructor 

// - 

Converter : : -Converter ( ) 

{ 

stop()  ; 

delete  []  doubleBuf fer ? 
delete  []  channelVector; 
delete  []  gainVector; 
cout  «M Converter  deleted "«endl  ; 
}//  end  destructor 


// - 

//  Function:  initConverter ( ) 

//  Return  Value:  None 
//  Parameters:  None 

//  Description:  Initialize  the  converter 

// - 

void  Converter :: initConverter ( ) 

{ 

//  init  channels  with  physical  numbers  on  the  board 

channelVector [ 0 ] =0 ; 

channelVector [ 1 ] =1 ; 

channelVector [2 ] =2 ; 

channelVector [3 ] =3 ; 

channelVector [ 4 ] =4 ; 

channelVector [ 5 ] =5 ; 

channelVector [6]=6; 

channelVector [7] =7; 

channelVector [ 8 ] =8 ; 

//  init  gains  for  channels 
for  (int  i=0; i<numberOf Channels ; i++) { 
gainVector [ i ] =gain ; 

} 

//  Configure  converter  to  software  triggered  and  to 
//  use  on  board  clock 

status  =  DAO  Config  (deviceNumber  ,  0,  0) ; 
if  (status  ==  0){ 

cerr  <<"  Converter  conf igured"«endl ; 

}  else  { 

cerr  «"Error  :  Conf igure\nM  ; 

} 

//  set  AI  values,  they  could  be  changed  by 
//  the  config  utility  software 

status  =  AI_Configure  (deviceNumber,  -1,  inputMode,  inputRange, 

polarity,  drive) ; 

if  (status  ==  0)  { 

cerr  <<"AI  conf iguredH<<endl ; 


116 


}  else  { 

cerr  «"Error  :  AI_configure\n" ; 

} 

//  Configure  the  time  out 

status  =  Timeout_Conf  ig  (deviceNumber ,  timeOut); 
if  (status  ==  0)  { 

cerr  «"time  out  set"«endl; 

}  else  { 

cerr  «"  Error  :  Time  out\n" ; 

} 

//  Enable  double  buffering  operations 

status  =  DAQ_DB_Conf  ig  (deviceNumber,  dBEnable)  ; 

if  (status  ==  0){ 

cerr  «"AD  Converter  initialized"  «endl ; 

}  else  { 

cerr  «status<<  "  Problem  with  initialization^" ; 

} 

cout  «"Converter  :  Initialized\n" ; 
return  ? 

}//end  initConverter ( ) 


// 


// 


Function: 

calibrate ( ) 

Return  Value: 

None 

Parameters : 

None 

Description: 

Calibrates  the  converter 

void  Converter: 

{ 


:  calibrate ( ) 


//  calibrates  the  converter 

status  =  Calibrate_E_Series (deviceNumber,  ND_SELF_CALIBRATE, 

ND_U  S  ER_E  E  PROM_AREA ,  0.0); 

if  (status  ! =  0){ 

cout  « " Error  Calibrated"  ; 
cout  «status; 

}  else{ 

cout  << " \nConverter  :  AD  Converter  calibrated"; 

} 


//  calculates  and  sets  the  rate 

status  =  DAQ_Rate (rate,  units,  &sampleT_Base,  &samplelnterval) ; 
if  (status  1=  0){ 

cerr  «"Error  DAQ_Rate\n"  ; 
cerr  «status; 

}else  { 

cerr  «"\nRate  calculated  \n"; 

} 

status  =0; 
return  ; 

}//end  calibrate () 
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// - 

//  Function:  printlnitialValues ( ) 

/ /  Return  Value :  None 
/ /  Parameters :  None 

//  Description:  Prints  the  initial  values 

// - 

void  Converter: : printlnitialValues ( ) 

{ 

cerr« " Channel '  s  gain  is  :  M«gain«endl ; 

switch  (inputMode) { 
case  0 : 

cerr<<"Mode  :  Dif ferential\nM ; 

break; 
case  1: 

cerr«"Mode  :  Referenced  Single  Ended\n" ; 

break; 
case  2 : 

cerr«"Mode  :  Non  Referenced  Single  Ended\n" 

break ; 
default : 

cerr« " Unknown  Mode  \n ”  ; 

break; 

} 

switch  (polarity) { 
case  0 : 

cerr  «"Polarity  :  Bipolar  -10/+10\n"; 

break ; 
case  1: 

cerr«"Polarity  :  Unipolar  0/  +  10\n"  ; 

break; 
default : 

cerr«" Unknown  polarity\n"  ; 

break; 

} 

cerr  «" Device  number  :  "«deviceNumber 

<<" \nNumber  of  Channels  :  " <<numberOf Channels 

«i.  \nTransfer  buffer  depth  :  "«  (buf ferSize/ (2*  numberOf Channels)  ) 
<<” \nDouble  buffer  depth  :  "  « (buf ferSize /numberOf Channel s ) 
«"  \nDouble  buffer  size  :  "«  buf  ferSize 

<<" \nSample  Time  Base  :  M«sampleT_Base 

«"  \nSample  Interval  :  "«samplelnterval 

<<" \nSampling  rate  :  " «rate/ numberOf Channels 

«" \nTime  out  :  ”<<timeOut<<endl ; 

return; 

}  //  end  printlnitialValues ( ) 
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// - 

//  Function:  getConf igData ( ) 

//  Return  Value:  None 

/ /  Parameters :  None 

//  Description:  Pass  the  values  to  the  sampler 

// - 

void  Converter :: getConf igData (unsigned  short  Grange, 

unsigned  short  ScmyGain,  int  &hal f Depth, int 
&block,  double  &  deltaTime) 

{ 

double  tBase  =  0.0, nano  =  1/1000000000.0,  micro  =  1/1000000.0; 
range  =  maxRange; 
myGain  =  gain; 

halfDepth  =  (bufferSize  /  (numberOfChannels*2 ) ) ; 
block  =  numberOf Channels ; 
if  (sampleT_Base  ==  -3 ) { 
tBase  =  50  *  nano; 

}else  if  (sampleT_Base  ==  -1){ 
tBase  =200  *  nano; 

}else  if  (sampleT_Base  ==  1) { 
tBase  =  micro; 

}else  if  (sampleT_Base  ==  2) { 
tBase  =  10  *  micro; 

}else  if  (sampleT_Base  ==  3) { 
tBase  =  100  *  micro; 

}else  if  (sampleT_Base  ==  4) { 
tBase  =  0.001; 

}else  if  (sampleT_Base  ==  5) { 
tBase  =  0.01; 

}else  { 

cerr  «  "ERROR  Converter  =  Unknown  sample  time  base\n"; 
tBase  =  0.0; 

}//  deltaTime  =  9  *  halfDepth  *  tBase; 
deltaTime  =  9  *  halfDepth  /  rate; 
cerr«"Update  rate  :  "«  deltaTime<<"  seconds\n"  ; 
return; 

}  //  end  getConf igData ( ) 

// - 

//  Function:  start () 

//  Return  Value:  None 
/ /  Parameters :  None 

//  Description:  Starts  the  data  acquisition  process 

// - 

void  Converter : : start ( ) 

{ 

//  set  the  scanning  values 

status  =  SCANjSetup  (deviceNumber ,  numberOf Channels  , 

channelVector,  gainVector) ; 

if  (status  ! =  0) { 

cerr  «status«"  Error  SCAN_Setup\n"  ,* 

} 

//  start  the  scanning  process 

status  =  SCAN_Start  (deviceNumber  ,  doubleBuf fer ,  bufferSize, 
sampleT_Base,  samplelnterval ,  scanT_JBase,  scanlnterval) ; 
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if  (status ! =0 ) { 

cout  «status<<"  Error  SCAN_Start\n" ; 

} 

status  =  0; 

cerr  « " \nConverter  :  started\n" ; 
return  ; 

}//end  start () 


// - 

//  Function:  stop() 

/ /  Return  Value :  None 
//  Parameters:  None 

//  Description:  Stops  the  converter 

// - 

void  Converter :: stop () 

{ 

status  =  DAQ_Clear (deviceNumber) ; 
status  =  DAQ_DB__Conf ig  (deviceNumber  ,  0); 
status  =  Timeout_Config  (deviceNumber  ,  -1)  ; 
status  =  0; 

cout  <<MConverter  :  Stoped  and  cleared"«endl ; 
return  ; 

}//end  stop() 

// - 

//  Function:  setEvents ( ) 

//  Return  Value:  None 
/ /  Parameters :  None 

//  Description:  Sets  the  events  for  converter 

// - 

void  Converter : : setEvents ( ) 

{  //no  events  set 

return  ; 

} / /end  setEvents ( ) 

// - 

//  Function:  isHal f Ready ( ) 

//  Return  Value:  true  if  half  ready 

/ /  Parameters :  None 

//  Description:  First  step  to  receive  data  from  A/D  converter 

// - 

bool  Converter: :isHal f Ready () 

{ 

ready  =  false; 

//  check  status 
half Ready  =0; 

while  ( ! ( (halfReady  ==  1)  &&  (status  ==  0))){ 

//  check  for  half  ready 

status  =  DAQ_DB_Hal f Ready  ( deviceNumber ,  &halfReady,  ScdagStop)  ; 
if  (status  ! =  0)  { 

cerr«"Half  ready  error  "«endl  ; 
restart ( ) ; 

} 

} 
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ready  =  true; 
return  ready; 
}//end  isHalf ready ( ) 


// - 

//  Function:  transferData ( ) 

//  Return  Value:  None 
/ /  Parameters :  None 

//  Description:  Transfers  the  half  buffer  to  transfer  buffer 

// - 

void  Converter :: transferData (short  *  trBuf) 

{ 

//  transfer  data 

status  =  DAQJDBJTransfer (deviceNumber, trBuf , 

kpointsTransfer,  ScdaqStop)  ; 

if  (status  ! =  0){ 

cerr«" Transfer  error "«endl; 
restart ( ) ; 

} 

return  ; 

}//end  transferData () 


// - 

//  Function:  restart () 

//  Return  Value:  None 
//  Parameters:  None 

//  Description:  Restarts  the  converter 

// - - 

void  Converter : : restart ( ) 

{ 

cerr«”  RESTARTING  Converter  error  status  :  "«status«endl ; 
status  =  DAQ_C 1 ear (deviceNumber) ; 
start ( ) ; 
return; 

}  //  end  restart () 


// - 

//  Function:  (friend)  operator«  ( ) 

//  Return  Value:  ostream  object 
//  Parameters:  output  -  ostream  object 

//  C  -  Converter  to  print 

//  Description:  Prints  the  Converter  information 

/j - 

ostream  &operator« (ostream  ^output,  const  Converter  &C) 

{ 

output  «"\nConverter  :  depth=  "«C  .buf ferSize 

«endl ; 

return  output; 

}  /  /  end  operator« 

//end  of  file  Converter . cpp 
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G 


SAMPLER.H 


//*******************★***★*****★****★*********★*★******************** 

//It****************************************************************** 

// 

//  File  :  Sampler. h 
//  Author  :  ildeniz  DUMAN 

//  Project  Name  :  Quaternion  Attitude  Estimation  Filter 
//  Operating  Environment  :  MS-Windows  95 
//  Compiler  :  Visual  C++  5.0 
//  Date  :  01/09/99 
// 

//  Description  : 

// 

//  -  Source  file  for  Sampler 
//  -  Receives  data  from  A/D  converter 
//  -  Averages,  scales  and  converts  this  data 
//  -  Updates  measurements  with  these  data 
// 

//  Inputs  :  None 
// 

//  Outputs  :  None 
// 

//  Process  :  None 
// 

//  Assumption  :  None 
// 

//  Warnings  during  compliation  :  None 
//********************★****★***************************************** 
//******★**★********************************************************* 

#ifndef  _ SAMPLER_H _ 

#def ine  _ SAMPLER_H _ 

#include  <iostream.h> 

# include  < iomanip . h> 
tinclude  " converter . h" 

#def ine  M_PI  3.14159265358979323846 

#define  GRAVITY  32.2185  //feet  per  second 


class  Sampler{ 

//  overloaded  operator« 

friend  ostream  &operator« (ostream  &, const  Sampler  &) ; 
public : 

//  default  constructor 
Sampler  (Converter  *); 

//  destructor 
-Sampler ( ) ; 
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//  finds  zero  level  voltages  for  calibration 
void  findZeroVoltages (bool)  ; 

//  get  data  from  AD  converter 
void  getData  (double  *,  double  Sc)  ; 

private : 


//AD  converter 
Converter  *  myAD; 


int  bufferDepth, 

// 

transfer  buffer  depth 

blockSize, 

// 

block,  row  size 

buf f erSize, 

// 

transfer  buffer  size 

count ; 

// 

number  of  readings 

unsigned  short  maxReading,  //  A/D  converter  max  reading 

adGain;  //  A/D  converter  channel  gain 

//  variables  used  for  conversions 

double  angXConversion  , angZConversion  , angYConversion  , 
accXConversion, accYConversion, accZConversion, 
magXConvers ion , magYConversion , magZConversion , 
angMultiplier, accMultiplier,magMultiplier , 
sampleTime,  //  sample  time 

//  zero  level  voltage  values 
angXV,  angYV,  angZV,  accXV, 
accYV^ccZV,  magXV,  magYV,  magZV, 

//  scalar  numbers  for  calibration 

angXScalar,  angYScalar,  angZScalar,  accXScalar,  accYScalar, 
accZScalar,  magXScalar,  magYScalar,  magZScalar, 

//  voltage  range  for  converter 
vol t Range; 

short  *  transferBuf fer ;  //  transfer  buffer 

//  reads  transfer  buffer,  updates  readings 
void  readAndUpdate (double  *); 

//  prints  the  transfer  buffer 
void  printTransferBuf fer ( ) ; 

//  converts  readings 

double  convert (double  ,  const  int) ; 

//  helper  function  of  convert ( ) 

double  convertHelper  (double  ,  double  ,  double) ; 


} ; 


#endif 

//  end  of  file  Sampler. h 
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H.  SAMPLER.CPP 


//*******★***************************★******************************* 

//*★***********★★*****★*******★******************★**********★******** 

// 

//File  :  Sampler. cpp 
//  Author  :  ildeniz  DUMAN 

//  Project  Name  :  Quaternion  Attitude  Estimation  Filter 
//  Operating  Environment  :  MS-Windows  95 
//  Compiler  :  Visual  C++  5.0 
//  Date  :  01/09/99 
// 

//  Description  : 

// 

//  -  Source  file  for  Sampler 
//  -  Receives  data  from  A/D  converter 
//  -  Averages,  scales  and  converts  this  data 
//  -  Updates  measurements  with  these  data 
// 

//  Inputs  :  None 
// 

//  Outputs  :  None 
// 

//  Process  :  None 
// 

//  Assumption  :  None 
// 

//  Warnings  during  compliation  :  None 
//******************************************★************************ 
//*******★********★*************★★*********★★*★★****************★**** 

#include  <assert.h> 

#include  <iomanip.h> 

#include  <f stream. h> 

# include  <sys/timeb.h> 

#include  <stdio.h> 

#include  <stdlib.h> 

#include  <time.h> 
tinclude  " sampler . h" 

#include  "util .h" 

# include  " converter . h" 


// - 

//  Function:  Sampler ( ) 

//  Return  Value:  None 

/ /  Parameters :  None 

//  Description:  Default  constructor 

// - 

Sampler :: Sampler  (Converter  *  con) 
:myAD(con) ,  //  my  AD  Converter 

count(O),  //  number  of  samples 

maxReading ( 0 ) ,  / /  max  converter  reading 

adGain(O),  //  channel  gain 

voltRange (10 . 0) ,  //  converter  volt  range 
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angXV (2 .573) , angYV (1.65) , angZV (2.21) ,  //  assign  zero  voltage  levels 
accXV (2.515) , accYV (2.45) , accZV (2.5) ,  //  also  will  be  calculated 

magXV(2 .45)  ,magYV(2 . 51)  /inagZV(2 . 509)  ,  //  by  f  indZeroVoltages  ( ) 

angXScalar (1.0) ,angYScalar (1.0) ,angZScalar (1.0) ,  //  scalar  numbers 
accXScalar (2 .99) , accYScalar (2 . 0) , accZScalar (1 . 0) ,  //  for  calibration 
magXScalar (1.5),  xnagYScalar (1.8), magZScalar (1.8) 

//  2.99  2.99  2.99  2.0  2.0  1.415  4.0  4.0  4.0  tested  scalars 

{ 

myAD->getConf igData (maxReading,  adGain,  buf ferDepth,  blockSize, 
sampleTime) ; 

bufferSize  =  buf ferDepth  *  blockSize; 
transferBuf fer  =  new  short  [bufferSize]; 
assert  (transferBuf fer  !=  0)  ; 
for  (int  k=0;k<bufferSize;k++) { 
transferBuf fer [k]  =  0; 

} 

angMultiplier  =  (90 . 0* (M_PI/180 . 0) )  ; 
accMultiplier  =  (4  *  GRAVITY); 
magMultiplier  =2.0  ; 

}//end  Constructor 


// 

// 

// 

// 

// 

// 


Function: 

-Sampler ( ) 

Return  Value : 

None 

Parameters : 

None 

Description: 

Destructor 

Sampler : : -Sampler ( ) 

{ 

cerr«"  deleting  sampler "  «endl  ; 
delete  []  transferBuf fer; 

}//  end  destructor 


// - 

//  Function: 

//  Return  Value: 
//  Parameters: 

//  Description: 
// 

// - 


f indZeroVoltages ( ) 

None 

selfCalibrate  -  bool  -  to  make  self  calibration 
Finds  the  zero  level  voltages,  gets  data  from 
converter  as  many  as  repeat  number 


void  Sampler :: f indZeroVoltages (bool  selfCalibrate) 

{ 

if  (selfCalibrate) { 

const  int  REPEAT  =  5; 

double  values [9] ={0 . 0} ,  zeros [REPEAT] [6] ,  total=0.0; 
long  sum  =  0; 


for  (int  r=0;r<REPEAT;r++) { 


//  check  if  half  buffer  is  ready 
while  ( !myAD->isHalfReady ( ) ) { ; } 
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} 


//call  transfer  function  of  AD 
myAD->transferData ( transf erBuf fer ) ; 

//  find  the  averaged  values 
for  (int  i=0 ; i<blockSize; i++) { 

for  (int  j  =  0 ; j <buf fer Size ;  j+=blockSize) { 
sum  +=  transf erBuf fer [j+i ]  ; 

} 

values [i]  =  (sum/ (buf ferDepth*l .  0)  )  ; 
sum  =  0 ; 

} 

//  fill  zero  level  voltages  matrix 
for  (int  t=0 ; t<6 ; t++ ) { 

zeros[r] [t]=  voltRange  *  values [t]  / 
(maxReading  *  adGain) ; 

} 

zeros [r] [5]  *=  (accMultiplier  / 

(accMultiplier  +  GRAVITY) ) ; 


//  find  averaged  zero  level  voltages 
for  (int  k=0;k<6;k++) { 

for  (int  rr=0 ; rr<REPEAT ; rr++ ) { 
total  +=  zeros [rr] [k] ; 

} 

zeros  [  0  ]  [k]  =  total  /REPEAT ; 
total=0 . 0 ; 

} 

//  assign  zero  level  voltages 

angXV  =  zeros [0][0]; 

angYV  =  zeros [0][1]; 

angZV  =  zeros [0] [2]; 

accXV  =  zeros [0]  [3]; 

accYV  =  zeros [0] [4]; 

accZV  =  zeros [0] [5]; 

/* 

for  (int  tt=0 ; tt<6 ; tt++) { 

cerr« "  "«zeros  [0]  [tt]  ; 

} 

*/ 

cerr«" Sampler  calculated  zero  level  voltages,  by  sampling" 
<<REPEAT<< "  times \n\nM ; 

}  else  {  //  use  hard  coded  values 

cerr« "  \nSampler  is  using  hard  coded  values  to  calibrated"  ; 

} 

//  assign  conversion  values  with  zero  level  voltages 
angXC onver s i on  =  maxReading  *  angXV  *  adGain  /  voltRange;//  2.573 

angYConversion  =  maxReading  *  angYV  *  adGain  /  voltRange;  //  1.65 

angZConversion  =  maxReading  *  angZV  *  adGain  /  voltRange;  //  2.21 

accXConver s ion  =  maxReading  *  accXV  *  adGain  /  voltRange;  //  2.51 
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accYConversion  =  maxReading  *  accYV  *  adGain  /  voltRange;  //  2.46 

accZConversion  =  maxReading  *  accZV  *  adGain  /  voltRange;  //  2.48 

magXConversion  =  maxReading  *  magXV  *  adGain  /  voltRange; 

magYConversion  =  maxReading  *  magYV  *  adGain  /  voltRange ; 

magZConversion  =  maxReading  *  magZV  *  adGain  /  voltRange; 

return; 

}  //  end  f indZeroVoltages ( ) 

// - 

//  Function:  getDataO 

//  Return  Value:  None 

//  Parameters:  double  m[]  -  measurements  array  from  Filter 

//  double  &  -  delta  t 

//Description:  Communicates  with  converter,  receives  data  into 

//  transfer  buffer,  avarages,  scales,  converts  and 

//  updates  measurements 

// - 

void  Sampler : :getData  (double  *  m  ,  double  &  delta_T) 

{ 

delta_T  =  sampleTime; 

//  errors  will  be  taken  care  of  by  Converter 
while  ( ! myAD->isHalf Ready ( ) ) { ; } 

//call  transfer  function  of  AD 
myAD->transferData (transferBuf fer) ; 

//printTransferBuf fer ( ) ; 
readAndUpdate (m) ; 
return; 

}//end  getDataO 


// - 

/ /  Function :  readAndUpdate ( ) 

//  Return  Value:  None 

//  Parameters:  double  ma[]  -  same  array  with  the  getDataO 

//  Description:  Reads  transfer  buffer,  avarages  data  and  updates 

//  measurements 

// - 

void  Sampler :: readAndUpdate (double  *  ma) 

{ 

long  sum  =  0; 

for  (int  i=0; icblockSize; i++) { 

for  (int  j=0; jcbuf ferSize;  j+=blockSize) { 
sum  +=  transferBuf fer [j+i] ; 

} 

ma[i]  =  convert ( (sum/ (buf ferDepth*! . 0 ) )  ,  i) ; 

sum  =  0 ; 

} 

return; 

} / /  end  readAndUpdate ( ) 
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// 

// 

// 

// 

// 

// 

// 

// 


Function: 
Return  Value : 

convert ( ) 

converted  value  -  double 

Parameters : 

averaged  -  double  -  averaged  value 
who  -int  -  related  reading  no 

Description : 

Generic  function,  converts 
using  related  numbers 

the  averaged  value  by 

double  Sampler :: convert (double  averaged  ,  const  int  who) 

/ 

double  result  =  0.0; 
switch  (who) { 

case  0:  //  body  rate  p 

result  =  convertHelper (averaged,  angXConversion, 
angMultiplier  *  angXScalar) ; 

break; 

case  1:  //  body  rate  q 

result  =  convertHelper (averaged,  angYConversion, 
angMultiplier  *  angYScalar) ; 

break ; 

case  2:  //  body  rate  r 

result  =  -convertHelper (averaged,  angZConversion, 
angMultiplier  *  angZScalar) ; 

break; 

case  3:  //  accelerometer  hi 

result  =  -convertHelper (averaged,  accXConversion  , 
accMultiplier  *  accXScalar) ; 

break; 

case  4:  //  accelerometer  h2 

result  =  -convertHelper (averaged,  accYConversion  , 
accMultiplier  *  accYScalar) ; 

break; 

case  5:  //  accelerometer  h3 

result  =  -convertHelper (averaged  ,  accZConversion  , 
accMultiplier  *  accZScalar) ; 

break; 

case  6:  //  magnetometer  bl 

result  =  -convertHelper (averaged  ,  magXConversion, 
magMultiplier  *  magXScalar) ; 

break ; 

case  7:  //  magnetometer  b2 

result  =  -convertHelper (averaged  ,  magYConversion, 
magMultiplier  *  magYScalar) ; 

break; 

case  8:  //  magnetometer  b3 

result  =  -convertHelper (averaged  ,  magZConversion, 
magMultiplier  *  magZScalar) ; 

break; 

default: 

cout  «”Error  in  Sampler:  Check  your  data  block  size\n"; 
break ; 

} 

return  result; 

} / /  end  convert ( ) 
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// - 

//  Function: 

//  Return  Value: 

/ /  Parameters : 


//  Description: 
// 


convertHelper ( ) 
converted  value 

theNumber  -  double  -  averaged  value 
adConversion  -  double  -  related  conversion  number 
mult  -  double  -  related  multiplier  contains  scalar 
Generic  function,  converts  the  averaged  value  by 
using  related  numbers 


double  Sampler : -.convertHelper  (double  theNumber  ,  double  adConversion, 

double  mult) 


return  (((theNumber  -  adConversion)  /  adConversion)  *  mult) ; 
} / /  end  convertHelper ( ) 


// - 

//  Function:  printTransferBuf fer ( ) 

//  Return  Value:  None 
//  Parameters:  None 

//  Description:  Prints  the  transfer  buffer 

// - 

void  Sampler: : printTransferBuf fer ( ) 

{ 

for  (int  i=0 ; i<buf ferDepth; i++) { 

for  (int  j=0; j<blockSize; j++) { 

cout«setprecision  (15)  «transferBuf  fer  [  j+  (i*blockSize)  ] «"  " 
} 

cout  «endl; 

} 

return; 

}//end  printTransferBuf fer ( ) 


// - 

//  Function:  (friend)  operator<< ( ) 

//  Return  Value:  ostream  object 

//  Parameters:  output  -  ostream  object 

//  s  -  sampler  to  print 

//  Description:  Prints  the  sampler  information 

// - 

ostream  &operator« (ostream  ^output,  const  Sampler  & s) 

{ 

output  «" \nsampler  :  depth=  "«s .buf ferDepth 

«"  block  size=  "«s  .blockSize 
«"  transfer  buffer  size=  "«s .buf ferSize 
« "  current  counter  =  "  «s  .  count «endl ; 

return  output; 

}//  end  operator« 

//end  of  file  Sampler. cpp 
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MATRIX.H 


//*********★*★*★*********★**★***★*★**********★***★*****************★★ 

//******************************************************************* 

// 

//  File  :  Matrix. h 
//  Author  :  ildeniz  DUMAN 

//  Project  Name  :  Matrix  Attitude  Estimation  Filter 
//  Operating  Environment  :  MS-Windows  95 
//  Compiler  :  Visual  C++  5.0 
//  Date  :  01/09/99 
// 

//  Description  : 

// 

//  -  Header  file  for  Matrix. cpp 
//  -  Executes  matrix  operations 
// 

//  Inputs  :  None 
// 

//  Outputs  :  None 
// 

//  Process  :  None 
// 

//  Assumption  :  None 
// 

//  Warnings  during  compliation  :  None 
//*****★***************★***★********************★****★**★***★******** 
//**********★*******************★***★**********★********************* 

#ifndef  _ MATRIX_H _ 

#def ine  MATRIX_H 

#include  <iostream.h> 

#include  <iomanip.h> 

#include  " quaternion . h" 


class  Matrix{ 

//  overloaded  operator« 

friend  ostream  &operator<< (ostream  &, const  Matrix  &) ; 
public : 

//  default  constructor 

Matrix  (char  *  mname =" Matrix " , int  mrow  =  4,int  mcol  =  4) ; 

//conversition  constructor  from  a  two  dimensional  double  array 
Matrix  (char  *  inname,  int  arrayRow,  int  arrayCol #  double  **)  ; 

//  destructor 
-Matrix ( ) ; 

//  copy  constructor 
Matrix  (const  Matrix  &) ; 
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/ /  matrix  invertion 
Matrix  invert { ) ; 

//  transpose 

void  transpose  (Matrix  &)  const; 

//  Matrix  product 

Matrix  operator* (const  Matrix  &)  const; 

//  Matrix  assignment 

Matrix  &operator= (const  Matrix  &) ; 

//  Matrix  product  and  assignment 
Matrix  &operator*= (const  Matrix  &) ; 

//  creates  a  unit  matrix 
Matrix  unitMatrix (int ) ; 

//  inserta  a  quaternion  in  a  row 

void  insertRowQuaterni on (Quaternion  &  a,  Quaternion  &  b,  int) 
//  inserta  a  quaternion  in  a  col 

void  insertColQuaternion (Quaternion  &  d,  Quaternion  &  c,  int) 

//  return  row  no 

int  getRow( ) {return  row;} 

//  return  col  no 

int  getCol ( ) {return  col; } 

//  returns  an  element  from  the  matrix 

double  getElement (int  i,  int  j){  return  matrix [i] [j ]; } 
private : 

//  the  name  of  the  Matrix 
char  *  name; 

//  the  elements  of  a  Matrix 
double  **  matrix; 

//row  and  column 
int  row,  col; 


} ; 


#endif 

//  end  of  file  Matrix. h 
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MATRIX.CPP 


//★★★★a************************************************************** 

//******************************************************************* 

// 

/ /  File  :  Matrix . cpp 
//  Author  :  ildeniz  DUMAN 

//  Project  Name  :  Matrix  Attitude  Estimation  Filter 
//  Operating  Environment  :  MS-Windows  95 
//  Compiler  :  Visual  C++  5.0 
//  Date  :  01/09/99 
// 

//  Description  : 

// 

//  -  Source  file  for  Matrix 
//  -  Executes  matrix  operations 
// 

//  Inputs  :  None 

// 

//  Outputs  :  None 

// 

//  Process  :  None 
// 

//  Assumption  :  None 
// 

//  Warnings  during  compliation  :  None 

//a****************************************************************** 

//*********★*****************************★*************************** 


# include 
#include 
# include 
#include 
# include 


cmath . h> 
<string.h> 
<assert .h> 
"Matrix. h" 

" quaternion. h" 


//- 

— 

// 

Function: 

Matrix ( ) 

// 

Return  Value: 

None 

// 

Parameters : 

mname  -  name  of  the  matrix 

// 

mrow  -  row  no 

// 

mcol  -  col  no 

// 

/  /- 

Description: 

Default  constructor 

Matrix: : Matrix  (char*  mname,int  mrow,  int  mcol) 
:  row  (mrow)  ,  col  (mcol ) 

{ 

int  length  =  strlen (mname) ; 
name  =  new  char [length+1 ] ; 
assert  (name  !=  0); 
strcpy (name, mname)  ; 

matrix  =  new  double  * [row] ; 
assert  (matrix  !=0); 
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for  (int  x  =  0;  x<row;  x++) { 

matrix [x]  =  new  double  [col]; 
assert  (matrix [x]  !=0) ; 

} 

for  (int  i  =  0 ; i<row;  i++) { 

for  (int  j  =  0 ; j <col ; j  ++) { 
matrix [i] [ j ]  =  0.0; 

} 


} 

}//end  Constructor 


// 


// 


Function: 

-Matrix ( ) 

Return  Value: 

None 

Parameters : 

None 

Description: 

Destructor 

Matrix : : -Matrix ( ) 

{ 


delete  []  name; 
for  (int  x=0 ;x<row; x++) { 
delete  matrix [x] ; 

} 

delete  []  matrix; 

}//end  destructor 


// 

// 

// 

// 

// 

// 


Function: 

Matrix (Matrix  &) 

Return  Value: 

None 

Parameters : 

MAT  -  Matrix  to  copy 

Description: 

Copy  constructor 

Matrix:  : Matrix  (const  Matrix  &MAT) 

{ 

int  length  =  strlen (MAT. name); 


name  =  new  char [length+1] ; 

assert (name  ! =  0) ; 

s trcpy  (name ,  MAT .  name )  ; 

matrix  =  new  double  * [MAT . row] ; 

assert  (matrix  !=0); 

for  (int  x  =  0;  x<MAT.row;  x++) { 

matrix [x]  =  new  double  [MAT. col ]; 
assert  (matrix [x]  !=0); 

} 

row=MAT .  row; 
col=MAT . col ; 

for  (int  i  =  0;i<MAT.row;i++) { 

for  (int  j  =  0 ; j <MAT . col ; j  ++ ) { 

matrix [i] [j]  =  MAT. matrix [i] [ j ] ; 

} 

} 

}//  end  copy  constructor 
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// - 

//  Function:  Matrix () 

//  Return  Value:  None 

//  Parameters:  mname  -  name  of  the  matrix 

//  arow  -  row  no 

//  acol  -  col  no 

//  double  [][]  -  a  -  two  dimesional  double  array 

//  Description:  Constructs  a  matrix  from  a  two  dimensional  array 

// - 

Matrix: rMatrix  (char  *  mname  ,  int  arow,  int  acol, double  **  a) 

{ 

int  length  =  strlen (mname)  ; 

name  =  new  char [length+1] ; 

assert  (name  !=  0); 

strcpy( name, mname)  ; 

matrix  =  new  double  * [arow] ; 

assert  (matrix  ! =0 ) ; 

for  (int  x  =  0;  x<arow;  x++) { 

matrix [x]  =  new  double  [acol]; 
assert  (matrix [x]  !=0); 

} 

row  =  arow; 
col  =  acol; 

for  (int  i  =  0 ; i<row;  i  +  +) { 

for  (int  j  =  0; j<col; j++) { 

matrix [i] [ j ]  =  a[i] [  j ] ; 

} 

} 

}//  end  Matrix () 


// - 

/ /  Function :  operator* ( ) 

//  Return  Value:  Matrix 

//  Parameters:  MAT  -  Matrix  &  -  Matrix  to  multiply 

//  Description:  Calculates  the  Matrix  product 

f / - 

Matrix  Matrix :: operator* (const  Matrix  &MAT)  const 

{ 

Matrix  dest ( " Product " ,  row  ,  MAT. col ); 

double  sum  =  O.Of; 

for  (int  i=0 ; i<row; i++) { 

for  (int  j  =  0 ; j <MAT . col ; j  ++ ) { 

for  (int  k=0;k<MAT.row;k++) { 

sum  +=  matrix [i]  [k]  *  MAT .matrix [k]  [j  ]  ; 

} 

dest .matrix[i] [j]=sum; 
sum  =  0.0; 

} 

} 

return (dest ) ; 

}//end  operator* 
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// - 

//  Function:  operator^ ( ) 

//  Return  Value:  Matrix  & 

//  Parameters:  MAT  -  Matrix  & 

//  Description:  Assigns  the  MAT  to  current  object 

// - 

Matrix  &  Matrix: : operator= (const  Matrix  &MAT) 

{ 

/*if  (&MAT  !=  this) { 
delete  []  name; 

int  length  =  strlen (MAT. name ) ; 
name  =  new  char [length+1 ] ; 
assert (name  1=  0); 
s trcpy  (name ,  MAT .  name )  ; 

} 

*/ 

//  I  let  self  assingment 
if  ( (row! =MAT.row)  ||  (col  !=  MAT.col)){ 
cout  «"Error  in  matrix  assignment 
}  else  { 

delete  []  name; 

int  length  =  strlen (MAT .name) ; 
name  =  new  char [length+1] ; 
assert (name  !=  0) ; 
s  trcpy  ( name ,  MAT .  name )  ; 

for  (int  i  =  0 ; i<MAT.row; i++) { 

for  (int  j  =  0; j<MAT.col; j++) { 

matrix[i][j]  =  MAT.matrix[i] [ j ] ; 

} 

} 

} 

return  (*this) ; 

}//end  operator= 

// - 

//  Function:  unitMatrix() 

//  Return  Value:  Matrix 

//  Parameters:  rowOrCol  -  int  -  square  matrix  index 

//  Description:  Creates  a  unit  matrix 

// - - - 

Matrix  Matrix: :unitMatrix  (int  rowOrCol) 

{ 

Matrix  Unit ( "unit" ,  rowOrCol  ,  rowOrCol); 
for  (int  i  =  0  ;  icUnit.row  ;  i++) { 

Unit .matrix [i] [i]  =  1.0; 

} 

return  (Unit) ; 

} / /  end  uni tMatrix ( ) 
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// - 

//  Function:  invert ( ) 

//  Return  Value:  Matrix 
//  Parameters:  None 

//  Description:  Calculates  the  matrix  inversion 

// - 

Matrix  Matrix: : invert ( ) 

{ 

double  multipliers . 0  ,  divider  =0.0; 

Matrix  myUnit=myUnit .unitMatrix (row) ; 

//  square  matrix  check 
if  (row  ==  col) { 

//inverting  the  matrix 
for  (int  j  =  0 ;  j<col;j++){ 

for  (int  i=0;  i<row;i++){ 
if  (i  ! =  j  ) { 

multiplier  =  -matrix [i ] [j ] /matrix [j ] [j ] ; 
for  (int  k=0;k<col;k++) { 

matrix [i] [k]  +=  (multiplier  * 
matrix  [j][k]); 

myUnit .matrix [i] [k]  +  =  (multiplier 
*  myUnit .matrix! j][k]); 


//  final  division  to  make  our  matrix  a  unit  matrix 
for  (int  i  =  0  ;  i<row  ;  i++) { 
divider  =  matrix [i] [i] ; 
if  (divider  !=  0.0) { 

matrix  [i] [i]  /=  matrix  [ i ] [ i ) ; 
for  (int  k=0; kcmyUnit .row;k++) { 

myUnit .matrix  [i] [k]  /=  divider; 

} 

} 

} 

}  else  { 

cout  «  "Error  :  Matrix  must  be  a  square  matrix  "«endl; 

} 

return  (myUnit) ; 

}//  end  unitMatrix () 
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// - 

//  Function:  operator*=() 

//  Return  Value:  Matrix 

//  Parameters:  MAT  -  Matrix  & 

//  Description:  Calculates  the  product  and  assigns  the  result  to 

//  current  object 

// - 

Matrix  Sc  Matrix: : operator*= (const  Matrix  &MAT) 

{ 

*this  =  *this  *  MAT; 
return  (*this) ; 

}//  end  operator *= 


// - 

//  Function:  transpose ( ) 

//  Return  Value:  None 

//  Parameters:  tr  -  Matrix 

//  Description:  Finds  the  transpose  of  a  matrix 

// - 

void  Matrix: : transpose (Matrix  &  tr)  const 

{ 

if  ((row  =-  tr.col)  &&  (col  ==  tr.row)){ 
for  (int  i=0;i<row;i++) { 

for  (int  j=0; jccol; j++) { 

tr . matrix [ j ] [i]  =  matrix [i] [ j ] ; 

} 

} 

} 

return; 

} / /  end  transpose ( ) 


// - 

/ /  Function :  insertRowQuaternion ( ) 

//  Return  Value:  None 

//  Parameters:  Q1  ,  Q2  -  Quaternions 

/ /  rowNo  -  int  -  row  no  to  insert 

//  Description:  Takes  two  quaternions  and  inserts  them  in  a  row  by 

//  ignoring  the  w  values  of  the  quaternions 

// - 

void  Matrix: : insertRowQuaternion  (Quaternion  &  Ql,  Quaternion  &  Q2 , 

int  rowNo) 

{ 

if  ((row  >=  rowNo)  &&  (col==6) ) { 

matrix  [rowNo]  [0]  =  Ql.getXO; 
matrix  [rowNo]  [1]  =  Ql.getYO; 
matrix  [rowNo]  [2]  =  Ql.getZO; 
matrix [rowNo] [3]  =  Q2.getX(); 
matrix [rowNo] [4]  =  Q2.getY(); 
matrix [rowNo] [5]  =  Q2 . getZ ( ) ; 

} 

return; 

} / /  end  insertRowQuaternion ( ) 
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// - 

//  Function: 

Return  Value: 
Parameters : 


// 

// 

// 

// 

// 

// 


Description: 


insertColQuaternion { ) 

None 

Q1  ,  Q2  -  Quaternions 

colNo  -  int  -  coloumn  no  to  insert 

Takes  two  quaternions  and  inserts  them  in  a  coloumn 
by  ignoring  the  w  values  of  the  quaternions 


void  Matrix: : insertColQuaternion  (  Quaternion  &  Q1 ,  Quaternion  &  Q2 , 

int  colNo) 


{ 


if  ( (col  >=  colNo)  && 
matrix [0] [colNo] 
matrix[l] [colNo] 
matrix [2] [colNo] 
matrix [3] [colNo] 
matrix [4] [colNo] 
matrix [5] [colNo] 

} 

return; 

}//end  insertColQuaternion ( ) 


(row==6) ) { 

=  Ql.getXO; 
=  Q1 . getY ( ) ; 
=  Q1 .getZ ( ) ; 
=  Q2 . getX ( ) ; 
=  Q2 . getY ( ) ; 
=  Q2 . getZ ( ) ; 


// 


// 

// 

// 

// 

// 


Function: 

(friend)  operator«  ( ) 

Return  Value: 

ostream  object 

Parameters : 

output  -  ostream  object 
q  -  Matrix  to  print 

Description: 

Prints  the  Matrix  in  a  form,  should  be  written  out 
of  class 

ostream  &operator« (ostream  ^output,  const  Matrix  &q) 

{ 

output  «*  [  ,«q.name«1  ]  ,«"  "«q.  row<<,,x,,«q. col<<endl ;  ; 


for  (int  k=0;k<q.row;k++) { 

for  (int  m=0;m<q.col;m++) { 

output  <<"  "<<q.matrix[k] [m] ; 

} 

output  «endl ; 

} 

return  output; 

}//  end  operator« 

//end  of  file  Matrix. cpp 
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K.  QUATERNION.H 


//******************************************************************* 

//if****************************************************************** 

// 

//  File  :  Quaternion. h 
//  Author  :  ildeniz  DUMAN 

//  Project  Name  :  Quaternion  Attitude  Estimation  Filter 
//  Operating  Environment  :  MS-Windows  95 
//  Compiler  :  Visual  C++  5.0 
//  Date  :  01/09/99 
// 

//  Description  : 

// 

//  -  Header  file  for  quaternion. cpp 
//  -  Executes  Qauternion  operations 
// 

/ /  Inputs  :  None 
// 

//  Outputs  :  None 
// 

//  Process  :  None 
// 

//  Assumption  :  None 
// 

//  Warnings  during  compilation  :  None 

//**************************★**************************************** 

//***********************************************************★*★***** 

#ifndef  _ QUATERNI ON_H _ 

# define  QUATERN I ON_H__ 

#include  ciostream. h> 

# include  <iomanip.h> 

#def ine  M_PI  3.14159265358979323846 


class  Quaternion{ 

//  overloaded  operator* 

friend  Quaternion  operator* (const  double, Quaternion  &) ; 

//  overloaded  operator« 

friend  ostream  &operator<< (ostream  &# const  Quaternion  &) ; 
public: 

//  default  constructor 

Quaternion  (char  *  qname =" Qua terni on ",  double  ww=0.0, 
double  xx=0 . 0, double  yy=0 . 0, double  zz=0.0); 

//  destructor 
-Quaternion ( ) ; 
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//  copy  constructor 
Quaternion  (const  Quaternion  &) ; 

//  get  data  members 
double  getW ( ) ; 
double  getX ( ) ; 
double  getY ( ) ; 
double  getZ ( ) ; 

//  quaternion  product 

Quaternion  operator* (const ‘Quaternion  &)  const; 

//  overwriting  operator* ( )  to  make  scalar  multiplications 
Quaternion  operator* (const  double); 

//  Quaternion  addition 

Quaternion  operator* (const  Quaternion  &)  const; 

//  Quaternion  substraction 

Quaternion  operator- (const  Quaternion  &)  const; 

//  quaternion  assignment 

Quaternion  &operator=  (const  Quaternion  Sc); 

II  quaternion  product  and  assignment 
Quaternion  &operator*= (const  Quaternion  &) ; 

//  quaternion  inverse  (conjugate) 

Quaternion  operator- ( )  const; 

//sets  the  quaternion 

void  setQuaternion (double  ,  double  ,  double  ,  double); 

//converts  to  Euler  Angles 
Quaternion  toEulerAngles ( ) ; 

//  rotate  a  quaternion  about  a  3D  vector  (w=0) 

Quaternion  rotation (const  Quaternion  &) ; 

//  converts  to  body  coordinates 
Quaternion  toBody (const  Quaternion  &) ; 

//  dot  product 

double  dotProduct (const  Quaternion  &) ; 

//  quaternion  to  axis  angles 
Quaternion  toAxisAngles () ; 

//  rotation  angles (w-0)  to  quaternion 
Quaternion  toQuaternion ( ) ; 

//  normalizes  a  quaternion 
void  normalize (); 
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//  calculates  partial  derivative 

Quaternion  partialDerivative  (const  Quaternion  Sc, 

const  Quaternion  Sc,  const  Quaternion  Sc)  const; 


private : 

//  the  name  of  the  quaternion 
char  *  name ; 

//  the  elements  of  a  quaternion 
double  w,x,y,z; 


} ; 


#endif 

//  end  of  file  Quaternion. h 
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L.  QUATERNION.CPP 


//*********★****★*****★*********★*******★***************★************ 

ZZ******************************************************************* 

// 

//File  :  Quaternion . cpp 
//  Author  :  ildeniz  DUMAN 

//  Project  Name  :  Quaternion  Attitude  Estimation  Filter 
//  Operating  Environment  :  MS-Windows  95 
//  Compiler  :  Visual  C++  5.0 
//  Date  :  01/09/99 
// 

//  Description  : 

// 

//  -  Source  file  for  quaternion 
II-  Executes  Qauternion  operations 
// 

//  Inputs  :  None 

// 

//  Outputs  :  None 

// 

//  Process  :  None 

// 

//  Assumption  :  None 

// 

//  Warnings  during  compliation  :  None 
//******************************************************************* 
//******************************************************************* 


#include 

#include 

#include 

#include 


<math.h> 
<string.h> 
<assert ,h> 

" Quaternion. h” 


/  /  ■ 

// 

Function: 

Quaternion ( ) 

// 

Return  Value: 

None 

// 

Parameters : 

None 

// 

Description : 

Default  constructor 

//■ 

- 

Quaternion: : Quaternion  (char*  qname, double  ww,double  xx.double  yy, 

double  zz) 

:w(ww)  ,  x  ( xx )  ,y  (yy)  ,  z  (zz) 

{ 

int  length  =  strlen (qname) ; 
name  =  new  char [ length+1 ]  ; 
assert  (name  !=  0) ; 
strcpy  (name,  qname)  ; 

}//end  Constructor 
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// - 

/ /  Function :  -Quaternion ( ) 

//  Return  Value:  None 

//  Parameters:  None 

//  Description:  Destructor 

// - 

Quaternion : : -Quaternion ( ) 

{ 

//cout  «" deleting  "«*this«endl; 

delete  []  name; 

}//end  destructor 


// - 

//  Function:  Quaternion (Quaternion  &) 

//  Return  Value:  None 

//  Parameters:  Quaternion 

//  Description:  Copy  constructor 

// - 

Quaternion: : Quaternion (const  Quaternion  &QUAT1 ) 

{ 

int  length  =  strlen(QUATl .name) ; 

name  =  new  char [length+1] ; 
assert (name  !=  0); 
strcpy  (name ,  QUAT1 .  name )  ; 
w  =  QUATl.w; 
x  =  QUATl.x; 
y  =  QUATl.y; 
z  =  QUATl.z; 

}//  end  copy  constructor 


// - - 

//  Function:  setQuaternion ( ) 

//  Return  Value:  None 

//  Parameters:  double  a,b,c,d  -  values  to  set 

//  Description:  Sets  the  quaternion  with  new  values 

// - 

void  Quaternion: : setQuaternion  (double  a,  double  b,  double  c,  double  d) 

{ 

w=a; 

x=b; 

y=c; 

z=d; 

return; 

) / /  end  setQuaternion ( ) 


//  get  functions  returns  the  data  members 
double  Quaternion: :getW( ) {return  w; } 
double  Quaternion: :getX( ) {return  x;} 
double  Quaternion: :getY() {return  y; } 
double  Quaternion: :getZ () {return  z;} 
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// - 

/ /  Function :  operator* ( ) 

//  Return  Value:  Quaternion 

//  Parameters:  QUAT  -  Quaternion  &  -  First  rotation 

//  Description:  Calculates  the  quaternion  product 

// - 

Quaternion  Quaternion :: operator* (const  Quaternion  &QUAT)  const 

{ 


Quaternion  dest ( "Quaternion  Product"); 


dest.w  = 

QUAT . w 

* 

w 

- 

QUAT . x 

★ 

X 

-  QUAT.y 

* 

y 

- 

QUAT . z 

★ 

z 

dest.x  = 

QUAT . w 

* 

X 

+ 

QUAT.x 

★ 

w 

-  QUAT.y 

★ 

z 

+ 

QUAT . z 

★ 

y 

dest.y  = 

QUAT . w 

* 

y 

+ 

QUAT . y 

★ 

w 

-  QUAT . z 

* 

X 

+ 

QUAT . x 

★ 

z 

dest.z  = 

QUAT . w 

★ 

z 

+ 

QUAT . z 

★ 

w 

-  QUAT.X 

★ 

y 

+ 

QUAT . y 

* 

X 

return (dest) ; 
}//end  operator* 


// - 

//  Function:  operator* (double)  overwriten 

//  Return  Value:  Quaternion 

//  Parameters:  NUM  -  double  -  a  scalar  to  multiply 

//  Description:  Calculates  the  scalar  product,  double  must  be  on 

//  the  right 

// - 

Quaternion  Quaternion: : operator* (const  double  NUM) 

{ 

w=NUM*w; 
x=NUM*x; 
y=NUM*y; 
z=NUM*z ; 
return  (*this) ; 

}//end  operator* (double) 


// - 

//  Function:  operator+ 

//  Return  Value:  Quaternion 

//  Parameters:  QUAT  -  Quaternion  -  a  quaternion  to  add 

//  Description:  Calculates  the  quaternion  addition 

// - 

Quaternion  Quaternion :: operator* (const  Quaternion  &QUAT)  const 

{ 

Quaternion  add ( "addition" ) ; 
add.w  =  w  +  QUAT .  w; 
add.x  -  x  +  QUAT.x; 
add.y  =  y  +  QUAT.y; 
add . z  =  z  +  QUAT . z ; 

return  (add) ; 

}  / /  end  operator+ ( ) 
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// - 

//  Function:  operator- 

//  Return  Value:  Quaternion 

//  Parameters:  QUAT  -  Quaternion  -  a  quaternion  to  substract 

//  Description:  Calculates  the  quaternion  substract ion 

// - 

Quaternion  Quaternion: : operator- (const  Quaternion  &QUAT )  const 

{ 

Quaternion  add (" subs tract ion" ) ; 

add.w  =  w  -  QUAT. w; 

add.x  =  x  -  QUAT.x; 

add.y  =  y  -  QUAT  .y; 

add . z  =  z  -  QUAT . z ; 

return  (add) ; 

} / /  end  operator- ( ) 


// - 

/ /  Func t i on :  opera tor = ( ) 

//  Return  Value:  Quaternion  & 

//  Parameters:  QUAT 2  -  Quaternion  & 

//  Description:  Assigns  the  QUAT2  to  current  object 

// - 

Quaternion  &  Quaternion: : opera tor= (const  Quaternion  &QUAT2) 

{ 

/*if  ( &QUAT2  ! =  this) { 
delete  []  name; 

int  length  =  strlen (QUAT2 .name) ; 
name  =  new  char [length+1] ; 
assert (name  !=  0); 
strcpy  (name ,  QUAT 2  .  name )  ; 

} 

I  let  self  assingment  */ 

W  =  QUAT 2  .w; 
x  =  QUAT2.X; 
y  =  QUAT2.y; 
z  =  QUAT2.Z; 

return  (*this) ; 

}//end  operator= 


// 


// 

// 

// 

// 

Quaternion  &  Quaternion: : operator*: 

{ 


Function: 

operator*= ( ) 

Return  Value: 

Quaternion 

Parameters : 

QUAT3  -  Quaternion  & 

Description: 

Calculates  the  product  and  assigns  the  result  to 
current  object 

(const  Quaternion  &QUAT3 ) 


*this  =  *this  *  QUAT 3 ; 
return  (*this) ; 

}//  end  operator* = 
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// - 

It  Function:  operator- ( ) 

//  Return  Value:  Quaternion 
//  Parameters:  None 

//  Description:  Calculates  the  inverse  (conjugate)  quaternion 

// - 

Quaternion  Quaternion :: operator- ( )  const 

{ 

Quaternion  temp ( "Conjugate" ) ; 

temp .  w  =  w; 

temp.x  =  -x; 

temp  .y  =  -y; 

temp. z  =  -z  ; 

return  (temp) ; 

}//end  operator- 


- 

//  Function: 

//  Return  Value: 
//  Parameters: 

//  Description: 
// - 


rotation ( ) 

Quaternion 

QUAT4  -  Quaternion  &  -  vector  to  rotate  around 
Calculates  the  rotation  quaternion 


Quaternion  Quaternion: : rotation (const  Quaternion  &QUAT4) 

{ 

Quaternion  temp ( "Rotation" ) ; 

temp  =  *this  *  (  QUAT4  *  (-(*this))); 

return  (temp) ; 

}//end  Rotation () 


// 

// 

// 

// 

// 

// 

// 


Function: 

toBody ( ) 

Return  Value: 

Quaternion 

Parameters : 

QUAT 4  -  Quaternion  &  - 
coordinates 

vector  to  convert  to  body 

Description: 

Converts  into  the  body 

coordinates 

Quaternion  Quaternion :: toBody (const  Quaternion  &QUAT) 

{ 

Quaternion  temp ( " to  Body" ) ; 

temp  =  ((-(*this))  *  (QUAT  *  (*this) ) ) ; 

return  (temp) ; 

}//end  Rotation () 
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// - - - - 

//  Function:  dotProduct ( ) 

//  Return  Value:  double 
//  Parameters:  QUAT4  -  Quaternion  & 

//  Description:  Calculates  the  dot  product  of  quaternions 

// - 

double  Quaternion: : dotProduct (const  Quaternion  &QUAT5) 

{ 

double  dotproduct; 

dotproduct  =  (w  *  QUAT5.W)  +  (x  *  QUAT5 .x)  +  (y  *  QUAT5 .y)  + 
(z  *  QUAT5.Z); 

return  (dotproduct) ; 

}  / /end  dotProduct ( ) 


// - 

//  Function:  normalize () 

//  Return  Value:  None 
/ /  Parameters :  None 

//  Description:  Normalizes  the  quaternion 

// - 

void  Quaternion: : normalize ( ) 

{ 

double  normal; 

normal  =  (x  *  x)  +  (y  *  y)  +  (z  *  z)  +  (w  *  w)  ; 
x  =  x  /  sqrt (normal) ; 
y  =  y  /  sqrt (normal) ; 
z  =  z  /  sqrt (normal) ; 
w  =  w  /  sqrt  (normal); 
return; 

}//end  normalize () 


// - 

//  Function:  toAxisAngles ( ) 

//  Return  Value:  Quaternion 

//  Parameters:  None 

//  Description:  Calculates  the  axis  angles  of  quaternion,  results 

//  must  be  in  glRotatefO 

// - 

Quaternion  Quaternion : : toAxisAngles ( ) 

{ 

Quaternion  axisAngle ( "To  Axis  Angle  "); 

double  scalarNumber , temp; 
temp  =  acos (w)  *  2.0; 
scalarNumber  =  sin(temp  /  2.0); 
axisAngle. x  =  x  /  scalarNumber; 

//  aligning  the  axes 

axisAngle. y  =  -1  *  z  /  scalarNumber; 
axisAngle. z  =  y  /  scalarNumber; 
axisAngle. w  =  (temp  *  (360  /  2.0))  /  M_PI; 
return  (axisAngle) ; 

}//end  toAxisAngles ( ) 
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// - 

//  Function:  toQuaternion ( ) 

//  Return  Value:  Quaternion 

//  Parameters:  None 

//  Description:  Calculates  the  quaternion  value  of  three  rotations 

// - 

Quaternion  Quaternion : : toQuaternion ( ) 

{ 

//current  object  is  a  3D  vector  with  rotation  angles  at  x,y,z 
double  radx , radY, radZ , tempx, tempy , tempz ; 

double  cosx, cosy , cosz , sinx, siny, sinz , cos c , coss, sine , sins ; 
Quaternion  quat ( "Euler  To  Quaternion"); 

//  convert  angles  to  radians 

radX  =  (x  *  M_PI)  /  (360.0  /  2.0); 

radY  =  (y  *  M_PI)  /  (360.0  /  2.0); 

radZ  =  (z  *  M_PI)  /  (360.0  /  2.0); 

//  half  angles 
tempx  =  radx  *  0.5; 
tempy  =  radY  *  0.5; 
tempz  =  radZ  *  0.5; 
cosx  =  cos (tempx); 
cosy  =  cos (tempy); 
cosz  =  cos (tempz); 
sinx  =  sin (tempx); 
siny  =  sin(tempy); 
sinz  =  sin(tempz); 

COSC  =  COSX  *  COSZ; 

coss  =  cosx  *  sinz; 

sine  =  sinx  *  cosz; 

sins  =  sinx  *  sinz; 

quat.x  =  (cosy  *  sine)  -  (siny  *  coss); 

quat.y  =  (cosy  *  sins)  +  (siny  *  cosc) ; 

quat.z  =  (cosy  *  coss)  -  (siny  *  sine) ; 

quat.w  =  (cosy  *  cosc)  +  (siny  *  sins); 

quat .normalize ( ) ; 
return (quat) ; 

}//  end  toQuaternion 

// - 

//  Function:  (friend)  operator* (double, Quaternion  &)  overwriten 

//  Return  Value:  Quaternion 

//  Parameters:  NUM  -  double  -  a  scalar  to  multiply 

//  Description:  Calculates  the  scalar  product 

// - 

Quaternion  operator* (const  double  NUM, Quaternion  &  quat) 

{ 

quat . w=NUM*quat . w; 
quat . x=NUM*quat . x; 
quat . y=NUM* qua t . y ; 
quat . z=NUM*quat . z ; 
return  (quat) ; 

}//end  operator* (double, Quaternion  &) 
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// 

// 

// 

// 

// 

// 

// 


Function: 

toEulerAngles ( ) 

Return  Value: 

Quaternion 

Parameters : 

None 

Description: 

Converts  quaternion  into  euler  angles  ,  this 
conversion  is  inherently  ill -defined 

Quaternion  Quaternion: : toEulerAngles { ) 

{ 

Quaternion  euler("Quat  to  euler"); 

double  sint , cost, sinv, cosv, sinf , cosf , ex,  ey,  ez ; 


sint  =  (2*w*y) - (2*x*z) ; 
cost  =  sqrt(l-  pow (sint , 2 ) ) ; 
if  (cost  ! =  0 . 0) { 

sinv  =  ( (2*y*z) + (2*w*x) ) /cost; 
cosv  =  (1- (2*x*x) - (2*y*y) ) /cost; 
sinf  =  (1- (2*x*x) - (2*y*y) ) /cost; 
cosf  =  (1- (2*y*y)  - (2*z*z) ) /cost; 
}else  { 

sinv  =  (2*w*x) - (2*y*z) ; 
cosv  =  1- (2*x*x) - (2*z*z) ; 
sinf  =  0.0; 
cosf  =  1.0; 

} 

ex  =  atan2 (sinv, cosv) ; 
ey  =  atan2 (sint , cost) ; 

ez  =  atan2 (sinf , cosf ) ;  //range  -pi  +pi 

ex  *=  180.0 /M_PI ; 
ey  *=  180 . 0/M_PI ; 
ez  *=  180.0/M_PI; 

euler . setQuaternion (0.0,  ex, ey , ez ) ; 
return  euler; 

}//end  toEulerAngles ( ) 


// - 

//  Function: 

//  Return  Value: 
/ /  Parameters : 

// 

// 

// 


//  Description: 
// 

// - 


partialDerivative ( ) 

Quaternion 

M  -  vector,  as  in  the  X  matrix 
QHAT  -  q-hat 

DER  -  partial  derivative  of  quaternion 
INVDER  -  inverse  partial  derivative  of  quaternion 
Calculates  the  partial  derivatives  for  X  Matrix, 
special  to  filter 


Quaternion  Quaternion: : partialDerivative (const  Quaternion  &  QHAT, 

const  Quaternion  &  DER, const  Quaternion  &  INVDER)  const 


{ 


return  ( (INVDER* ( (*this)  *  QHAT) )+( (-QHAT) *(( *this) *DER) )) ; 
}//  end  partialDerivative ( ) 
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// - 

//  Function:  (friend)  operator<<() 

//  Return  Value:  ostream  object 

//  Parameters:  output  -  ostream  object 

//  q  -  quaternion  to  print 

//  Description:  Prints  the  quaternion  in  a  form,  should  be  written 

//  out  of  class 

// - 

ostream  &operator« (ostream  ^output,  const  Quaternion  &q) 

{ 

/*  output  «q.name<<"  w=  "«q.w 


«" 

x= 

"<<q, 

,  X 

«" 

y= 

"«q. 

*y 

«’■ 

z  = 

"«q. 

.  z ;  *  / 

output  <<  q.w  <<”  "<<q.x<<"  "<<q.y<<"  " <<q.z<<"  " 
return  output; 

}//  end  operator« 

//end  of  file  Quaternion . cpp 
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M.  UTIL.H 


/  J******************************************************************* 
/I******************************************************************* 
// 

//  File  :  Util.h 
//  Author  :  ildeniz  DUMAN 

//  Project  Name  :  Quaternion  Attitude  Estimation  Filter 
//  Operating  Environment  :  MS-Windows  95 
//  Compiler  :  Visual  C++  5.0 
//  Date  :  01/09/99 
// 

//  Description  : 

// 

//  -  Header  file  for  Utilities 

// 

//  Inputs  :  None 

// 

//  Outputs  :  None 
// 

//  Process  :  None 

// 

//  Assumption  :  None 
// 

//  Warnings  during  compilation  :  None 

//******************************************************************* 

//*****************************************★************************* 

#ifndef  _ UTIL_H _ 

#def ine  UTIL_H 


#include  <iostream.h> 
#include  <f stream. h> 


//  opens  a  file  to  read 

int  openFile  (ifstream  &  ,char  *  ); 

//  opens  a  file  to  write 

int  writeFile  (of stream  &  , char  *  ); 

#endif 

//  end  of  file  Util.h 
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N, 


UTIL.CPP 


/  /  *  *  * **************************************************************** 
//******************************************************************* 
// 

//  File  :  Util.cpp 
//  Author  :  ildeniz  DUMAN 

//  Project  Name  :  Quaternion  Attitude  Estimation  Filter 
//  Operating  Environment  :  MS-Windows  95 
//  Compiler  :  Visual  C++  5.0 
//  Date  :  01/09/99 
// 

//  Description  : 

// 

//  -  Source  file  for  Utilities 

//  -  Opens  a  file  to  write  and  read 

// 

//  Inputs  :  None 
// 

//  Outputs  :  None 
// 

//  Process  :  None 
// 

//  Assumption  :  None 
// 

//  Warnings  during  compliation  :  None 

//************★************** **************************************** 

//****★***********************★************★*★*********************** 


ttinclude 
#include 
# include 
#include 


<iostream.h> 
<f stream. h> 
<iomanip.h> 
"util . h" 


// 

// 

// 

// 

// 

// 

// 


Function: 

openFile ( ) 

Return  Value: 

None 

Parameters : 

inDataFile  -  &  ifstream  -  ifstream  object 
filename  -  char  *  -  file  name  to  open 

Description: 

Opens  a  file  to  read 

int  openFile  (ifstream  &  inDataFile  ,char  *  fileName) 

{ 


int  error  =  0; 


inDataFile . open  (fileName  ,  ios::in); 

cout  <<"Reading  "«f ileName«"  for  configuration  settings\n" ; 
if  (! inDataFile) { 

cout  «"File  "«fileName«"  not  found\n"  ; 
error  =  1; 

} 

return  error; 

}//  end  openFile () 
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// 


// 

// 

// 

// 

int  writeFile (of stream  &  outDataFile,  char  *fileName) 

{ 

int  error  =  0; 


Function: 

writeFile ( ) 

Return  Value: 

None 

Parameters : 

inDataFile  -  &  if stream  -  if stream  object 
filename  -  char  *  -  file  name  to  open 

Description: 

Opens  a  file 

to  write 

outDataFile. open  (fileName  ,  ios::out); 
cout  «fileName«"  opened  to  write\n"; 

if  ( ! outDataFile) { 

cout  «"File  M«fileName<<"  not  found\n" ; 
error  =  1; 


return  error; 

}//  end  writeFile () 

//end  of  file  util.cpp 
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O.  TEST.CPP 


//******************************************************************* 

//*****************************************************★************* 

// 

//  File  :  test.cpp 
//  Author  :  ildeniz  DUMA N 

//  Project  Name  :  Quaternion  Attitude  Estimation  Filter 
//  Operating  Environment  :  MS-Windows  95 
//  Compiler  :  Visual  C++  5.0 
//  Date  :  01/09/99 
// 

//  Description  : 

// 

//  -  Source  file  for  test 

//  -  Sample  driver  file  for  the  filter 

// 

//  Inputs  :  None 

// 

//  Outputs  :  None 
// 

//  Process  :  None 

// 

//  Assumption  :  None 
// 

//  Warnings  during  compliation  :  None 
// 

//  Note  :  nidaq32.1ib/  nidaq.h  and  nidaqcns.h  must  be  added  to  the 
//  project  file 

//*****★********★*************★★*******★**★*****************★****★*★* 

//*************★*******★**★★***************************************** 


# include 
#include 
# include 
# include 


<math.h> 

" qaef .h" 

" quaternion. h" 
"util .h" 


int  main  ( )  { 

//  file  name  to  log  data 

char  *  f name ; 

fname  =  new  char[15]; 

//  delta  t 
double  dt=0.0; 

//  step  counter 
int  step  =1; 

//  stream  to  write  file 
ofstream  result; 
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//  quaternions 

Quaternion  drawQuat ( " d" , 0 . 0 ) ; 

Quaternion  qRotation ( M q" , 0 . 0 ) ; 

//  filter 

Qaef  myQaef (1,  40,  3); 

cerr«HEnter  file  name,  with  .dat  :  "  ; 
cin»fname; 

writeFile (result, fname) ; 

//  -start  filter  and  get  delta  t 
dt  =  myQaef .start () ; 

while (true) { 

//  get  estimated  result 
drawQuat  =  myQaef . getResult ( ) ; 

//  convert  into  angles 

qRotation  =  drawQuat . toAxisAngles ( ) ; 

//cerr«qRotation.getW()«"  " ; 

//  write  results  in  to  file 

result«  (step*dt) «"  M«qRotation«endl; 

step++; 

} 

//  stop  the  filter 
myQaef . stop ( ) ; 

return  ( 0 ) ; 

}  //  end  main() 

//  end  of  file  test.cpp 
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